СИСТЕМЫ УПРАВЛЕНИЯ
🎛️ МОДУЛЬ 6: “СИСТЕМЫ УПРАВЛЕНИЯ” (10 часов)
“От хаоса к порядку: создаем интеллектуальные системы управления полетом”
🧠 ФИЛОСОФИЯ СИСТЕМ УПРАВЛЕНИЯ
Эволюция от компонентов к системе:
1🔄 ПУТЬ ТЕХНИЧЕСКОГО МАСТЕРСТВА:
2
3МОДУЛЬ 4: "МЕХАНИК"
4- Понимание физики полета
5- Знание характеристик компонентов
6- Сборка и балансировка
7
8МОДУЛЬ 5: "ПРОГРАММИСТ"
9- Программирование микроконтроллеров
10- Работа с датчиками и протоколами
11- Создание базовых алгоритмов
12
13МОДУЛЬ 6: "СИСТЕМНЫЙ ИНЖЕНЕР"
14- Проектирование поведения системы
15- Настройка сложных алгоритмов управления
16- Создание интеллектуальных автопилотов
17- Диагностика и оптимизация производительности
Концепция “Думающего дрона”:
1🤖 ОТ РЕАКТИВНОГО К ИНТЕЛЛЕКТУАЛЬНОМУ:
2
3УРОВЕНЬ 1: РЕАКТИВНОЕ УПРАВЛЕНИЕ
4- Простая реакция на команды пилота
5- Базовая стабилизация
6- Фиксированные алгоритмы
7
8УРОВЕНЬ 2: АДАПТИВНОЕ УПРАВЛЕНИЕ
9- Автоматическая подстройка параметров
10- Компенсация внешних возмущений
11- Обучение на основе опыта
12
13УРОВЕНЬ 3: ПРЕДИКТИВНОЕ УПРАВЛЕНИЕ
14- Предвидение развития ситуации
15- Упреждающие корректировки
16- Оптимизация траекторий
17
18УРОВЕНЬ 4: ИНТЕЛЛЕКТУАЛЬНОЕ УПРАВЛЕНИЕ
19- Принятие решений высокого уровня
20- Планирование миссий
21- Взаимодействие с другими системами
📐 УРОК 1: ТЕОРИЯ УПРАВЛЕНИЯ НА ПРАКТИКЕ (2.5 часа)
1.1 PID регулятор: от математики к реальности
ИНТУИТИВНОЕ ПОНИМАНИЕ PID:
1🎯 PID БЕЗ СЛОЖНЫХ ФОРМУЛ:
2
3P - ПРОПОРЦИОНАЛЬНАЯ СОСТАВЛЯЮЩАЯ:
4Аналогия: Резиновая лента между желаемым и текущим положением
5
6Представь дрон на веревочке:
7- Большое отклонение → сильное "натяжение" → большая корректировка
8- Маленькое отклонение → слабое "натяжение" → слабая корректировка
9
10Практический эффект:
11✅ Быстрая реакция на ошибки
12❌ Может вызвать колебания
13❌ Не убирает постоянную ошибку
14
15I - ИНТЕГРАЛЬНАЯ СОСТАВЛЯЮЩАЯ:
16Аналогия: Накопительный "штраф" за долгую ошибку
17
18Представь копилку ошибок:
19- Ошибка есть → монетки накапливаются
20- Чем дольше ошибка → тем больше коррекция
21- Ошибка исчезла → копилка медленно пустеет
22
23Практический эффект:
24✅ Убирает постоянные ошибки (дрейф)
25✅ Компенсирует систематические возмущения
26❌ Может вызвать "раскачку"
27
28D - ДИФФЕРЕНЦИАЛЬНАЯ СОСТАВЛЯЮЩАЯ:
29Аналогия: Предсказатель будущего
30
31Представь провидца:
32- Ошибка растет быстро → "Стой! Будет хуже!"
33- Ошибка уменьшается → "Притормози коррекцию"
34- Предвидит куда движется система
35
36Практический эффект:
37✅ Сглаживает колебания
38✅ Повышает стабильность
39❌ Усиливает шум датчиков
ПРАКТИЧЕСКАЯ НАСТРОЙКА PID:
1// =====================================
2// ИНТЕРАКТИВНАЯ НАСТРОЙКА PID
3// =====================================
4
5class InteractivePIDTuner {
6private:
7 float kp, ki, kd;
8 float integral, lastError;
9 float setpoint, measurement;
10
11 // История для анализа
12 float errorHistory[100];
13 float outputHistory[100];
14 int historyIndex = 0;
15
16public:
17 InteractivePIDTuner() : kp(1.0), ki(0.1), kd(0.05) {}
18
19 float update(float target, float current, float dt) {
20 setpoint = target;
21 measurement = current;
22
23 float error = target - current;
24
25 // P составляющая
26 float proportional = kp * error;
27
28 // I составляющая
29 integral += error * dt;
30 integral = constrain(integral, -10, 10); // Anti-windup
31 float integralTerm = ki * integral;
32
33 // D составляющая
34 float derivative = (error - lastError) / dt;
35 float derivativeTerm = kd * derivative;
36 lastError = error;
37
38 float output = proportional + integralTerm + derivativeTerm;
39
40 // Сохранение истории для анализа
41 errorHistory[historyIndex] = error;
42 outputHistory[historyIndex] = output;
43 historyIndex = (historyIndex + 1) % 100;
44
45 return constrain(output, -1.0, 1.0);
46 }
47
48 void tunePID() {
49 Serial.println("=== ИНТЕРАКТИВНАЯ НАСТРОЙКА PID ===");
50 Serial.println("Команды:");
51 Serial.println("p+/p- : увеличить/уменьшить Kp");
52 Serial.println("i+/i- : увеличить/уменьшить Ki");
53 Serial.println("d+/d- : увеличить/уменьшить Kd");
54 Serial.println("r : сброс интегратора");
55 Serial.println("s : показать статистику");
56 Serial.println("a : автонастройка");
57
58 while (true) {
59 if (Serial.available()) {
60 String command = Serial.readString();
61 command.trim();
62
63 if (command == "p+") kp += 0.1;
64 else if (command == "p-") kp -= 0.1;
65 else if (command == "i+") ki += 0.01;
66 else if (command == "i-") ki -= 0.01;
67 else if (command == "d+") kd += 0.01;
68 else if (command == "d-") kd -= 0.01;
69 else if (command == "r") integral = 0;
70 else if (command == "s") showStatistics();
71 else if (command == "a") autoTune();
72 else if (command == "exit") break;
73
74 kp = max(0, kp);
75 ki = max(0, ki);
76 kd = max(0, kd);
77
78 Serial.printf("PID: P=%.2f I=%.3f D=%.3f\n", kp, ki, kd);
79 }
80
81 // Симуляция системы для настройки
82 static float simulatedPosition = 0;
83 static float simulatedVelocity = 0;
84 float targetPosition = 10.0 * sin(millis() / 3000.0); // Синусоида
85
86 float pidOutput = update(targetPosition, simulatedPosition, 0.02);
87
88 // Простая модель системы
89 simulatedVelocity += pidOutput * 0.02;
90 simulatedVelocity *= 0.95; // Трение
91 simulatedPosition += simulatedVelocity * 0.02;
92
93 Serial.printf("Target: %6.2f | Current: %6.2f | Error: %6.2f | Output: %6.2f\n",
94 targetPosition, simulatedPosition, targetPosition - simulatedPosition, pidOutput);
95
96 delay(20);
97 }
98 }
99
100 void showStatistics() {
101 // Анализ производительности
102 float avgError = 0, maxError = 0, oscillations = 0;
103
104 for (int i = 0; i < 100; i++) {
105 avgError += abs(errorHistory[i]);
106 maxError = max(maxError, abs(errorHistory[i]));
107 }
108 avgError /= 100;
109
110 // Подсчет колебаний
111 for (int i = 1; i < 99; i++) {
112 if ((outputHistory[i-1] < outputHistory[i]) && (outputHistory[i] > outputHistory[i+1])) {
113 oscillations++;
114 }
115 }
116
117 Serial.println("\n=== СТАТИСТИКА ПРОИЗВОДИТЕЛЬНОСТИ ===");
118 Serial.printf("Средняя ошибка: %.3f\n", avgError);
119 Serial.printf("Максимальная ошибка: %.3f\n", maxError);
120 Serial.printf("Колебания за 100 семплов: %.0f\n", oscillations);
121
122 // Рекомендации
123 if (avgError > 1.0) {
124 Serial.println("💡 Большая ошибка - увеличьте Kp");
125 }
126 if (oscillations > 20) {
127 Serial.println("💡 Много колебаний - уменьшите Kp или увеличьте Kd");
128 }
129 if (maxError > 0.5 && oscillations < 5) {
130 Serial.println("💡 Медленная реакция - увеличьте Kp или уменьшите Kd");
131 }
132 }
133
134 void autoTune() {
135 Serial.println("Автонастройка PID по методу Ziegler-Nichols...");
136
137 // Упрощенная автонастройка
138 float bestKp = 1.0;
139 float bestPerformance = 999;
140
141 // Поиск критического Kp
142 for (float testKp = 0.1; testKp <= 5.0; testKp += 0.1) {
143 kp = testKp;
144 ki = 0;
145 kd = 0;
146
147 float performance = testPerformance(3.0); // Тест 3 секунды
148
149 if (performance < bestPerformance) {
150 bestPerformance = performance;
151 bestKp = testKp;
152 }
153
154 Serial.printf("Тест Kp=%.1f, производительность: %.2f\n", testKp, performance);
155 }
156
157 // Установка оптимальных параметров по Ziegler-Nichols
158 kp = bestKp * 0.6;
159 ki = 2.0 * kp / 1.0; // Ti = 1 секунда
160 kd = kp * 0.125; // Td = 0.125 секунды
161
162 Serial.printf("Автонастройка завершена: P=%.2f I=%.3f D=%.3f\n", kp, ki, kd);
163 }
164
165private:
166 float testPerformance(float testDuration) {
167 // Тестируем производительность PID за заданное время
168 float totalError = 0;
169 int samples = testDuration * 50; // 50Hz
170
171 float testPosition = 0, testVelocity = 0;
172 integral = 0;
173 lastError = 0;
174
175 for (int i = 0; i < samples; i++) {
176 float target = 5.0; // Ступенчатое воздействие
177 float pidOut = update(target, testPosition, 0.02);
178
179 testVelocity += pidOut * 0.02;
180 testVelocity *= 0.95;
181 testPosition += testVelocity * 0.02;
182
183 totalError += abs(target - testPosition);
184 }
185
186 return totalError / samples;
187 }
188};
189
190// Пример использования
191InteractivePIDTuner pidTuner;
192
193void setupPIDTuning() {
194 pidTuner.tunePID();
195}
1.2 Многоконтурные системы управления
КАСКАДНЫЕ РЕГУЛЯТОРЫ:
1🏗️ АРХИТЕКТУРА МНОГОУРОВНЕВОГО УПРАВЛЕНИЯ:
2
3УРОВЕНЬ 1: УПРАВЛЕНИЕ ПОЛОЖЕНИЕМ
4┌─────────────────────────────────────┐
5│ POSITION CONTROLLER │
6│ Вход: Желаемая позиция (X, Y, Z) │
7│ Выход: Желаемые углы (Roll, Pitch) │
8│ Частота: 10-50 Hz │
9└─────────────────────────────────────┘
10 │
11 ▼
12УРОВЕНЬ 2: УПРАВЛЕНИЕ УГЛАМИ
13┌─────────────────────────────────────┐
14│ ATTITUDE CONTROLLER │
15│ Вход: Желаемые углы │
16│ Выход: Желаемые угловые скорости │
17│ Частота: 50-100 Hz │
18└─────────────────────────────────────┘
19 │
20 ▼
21УРОВЕНЬ 3: УПРАВЛЕНИЕ УГЛОВЫМИ СКОРОСТЯМИ
22┌─────────────────────────────────────┐
23│ RATE CONTROLLER │
24│ Вход: Желаемые угловые скорости │
25│ Выход: Команды моторам │
26│ Частота: 100-1000 Hz │
27└─────────────────────────────────────┘
ПРАКТИЧЕСКАЯ РЕАЛИЗАЦИЯ:
1// =====================================
2// КАСКАДНАЯ СИСТЕМА УПРАВЛЕНИЯ
3// =====================================
4
5class CascadeController {
6public:
7 struct Position {
8 float x, y, z;
9 };
10
11 struct Attitude {
12 float roll, pitch, yaw;
13 };
14
15 struct Rates {
16 float rollRate, pitchRate, yawRate;
17 };
18
19private:
20 // PID контроллеры для каждого уровня
21 InteractivePIDTuner positionXPID, positionYPID, positionZPID;
22 InteractivePIDTuner attitudeRollPID, attitudePitchPID, attitudeYawPID;
23 InteractivePIDTuner rateRollPID, ratePitchPID, rateYawPID;
24
25 // Ограничения между уровнями
26 float maxTiltAngle = 30.0; // Максимальный наклон ±30°
27 float maxYawRate = 180.0; // Максимальная скорость поворота ±180°/с
28 float maxRotationRate = 720.0; // Максимальная угловая скорость ±720°/с
29
30public:
31 CascadeController() {
32 // Настройка PID параметров для каждого уровня
33 setupPositionControllers();
34 setupAttitudeControllers();
35 setupRateControllers();
36 }
37
38 // Главная функция управления
39 void update(Position desiredPos, Position currentPos,
40 Attitude currentAtt, Rates currentRates,
41 float dt, float motorOutputs[4]) {
42
43 // УРОВЕНЬ 1: Управление позицией → желаемые углы
44 Attitude desiredAttitude = positionControl(desiredPos, currentPos, dt);
45
46 // УРОВЕНЬ 2: Управление углами → желаемые угловые скорости
47 Rates desiredRates = attitudeControl(desiredAttitude, currentAtt, dt);
48
49 // УРОВЕНЬ 3: Управление угловыми скоростями → команды моторам
50 rateControl(desiredRates, currentRates, dt, motorOutputs);
51 }
52
53private:
54 void setupPositionControllers() {
55 // Позиционные контроллеры - медленные, интегральные
56 // Настройки для плавного полета
57 }
58
59 void setupAttitudeControllers() {
60 // Угловые контроллеры - средняя скорость
61 // Баланс между точностью и стабильностью
62 }
63
64 void setupRateControllers() {
65 // Скоростные контроллеры - быстрые, без интегральной составляющей
66 // Максимальная отзывчивость
67 }
68
69 Attitude positionControl(Position desired, Position current, float dt) {
70 Attitude output;
71
72 // X позиция → Roll угол
73 float xError = desired.x - current.x;
74 float rollCommand = positionXPID.update(0, xError, dt);
75 output.roll = constrain(rollCommand, -maxTiltAngle, maxTiltAngle);
76
77 // Y позиция → Pitch угол
78 float yError = desired.y - current.y;
79 float pitchCommand = positionYPID.update(0, yError, dt);
80 output.pitch = constrain(pitchCommand, -maxTiltAngle, maxTiltAngle);
81
82 // Z позиция → Throttle (обрабатывается отдельно)
83 // Yaw остается от пилота или навигации
84 output.yaw = 0; // Или from external source
85
86 return output;
87 }
88
89 Rates attitudeControl(Attitude desired, Attitude current, float dt) {
90 Rates output;
91
92 // Roll угол → Roll rate
93 float rollError = desired.roll - current.roll;
94 output.rollRate = attitudeRollPID.update(0, rollError, dt);
95 output.rollRate = constrain(output.rollRate, -maxRotationRate, maxRotationRate);
96
97 // Pitch угол → Pitch rate
98 float pitchError = desired.pitch - current.pitch;
99 output.pitchRate = attitudePitchPID.update(0, pitchError, dt);
100 output.pitchRate = constrain(output.pitchRate, -maxRotationRate, maxRotationRate);
101
102 // Yaw угол → Yaw rate
103 float yawError = desired.yaw - current.yaw;
104 // Обработка перехода через ±180°
105 if (yawError > 180) yawError -= 360;
106 if (yawError < -180) yawError += 360;
107
108 output.yawRate = attitudeYawPID.update(0, yawError, dt);
109 output.yawRate = constrain(output.yawRate, -maxYawRate, maxYawRate);
110
111 return output;
112 }
113
114 void rateControl(Rates desired, Rates current, float dt, float motors[4]) {
115 // Контроль угловых скоростей → PWM моторов
116
117 float rollOutput = rateRollPID.update(desired.rollRate, current.rollRate, dt);
118 float pitchOutput = ratePitchPID.update(desired.pitchRate, current.pitchRate, dt);
119 float yawOutput = rateYawPID.update(desired.yawRate, current.yawRate, dt);
120
121 // Микширование на квадрокоптер (X конфигурация)
122 float throttle = 0.5; // Базовый газ (от altitude controller)
123
124 motors[0] = throttle + pitchOutput - rollOutput - yawOutput; // Front Right
125 motors[1] = throttle - pitchOutput - rollOutput + yawOutput; // Rear Right
126 motors[2] = throttle - pitchOutput + rollOutput - yawOutput; // Rear Left
127 motors[3] = throttle + pitchOutput + rollOutput + yawOutput; // Front Left
128
129 // Нормализация (если сумма превышает максимум)
130 float maxMotor = max({motors[0], motors[1], motors[2], motors[3]});
131 if (maxMotor > 1.0) {
132 float scale = 1.0 / maxMotor;
133 for (int i = 0; i < 4; i++) {
134 motors[i] *= scale;
135 }
136 }
137
138 // Ограничение минимума
139 for (int i = 0; i < 4; i++) {
140 motors[i] = constrain(motors[i], 0.0, 1.0);
141 }
142 }
143};
144
145// Пример интеграции с системой
146CascadeController flightController;
147
148void flightControlLoop() {
149 // Получение данных датчиков
150 CascadeController::Position currentPos = getCurrentPosition();
151 CascadeController::Attitude currentAtt = getCurrentAttitude();
152 CascadeController::Rates currentRates = getCurrentRates();
153
154 // Желаемая позиция (от навигации или пилота)
155 CascadeController::Position targetPos = getTargetPosition();
156
157 // Управление
158 float motorCommands[4];
159 flightController.update(targetPos, currentPos, currentAtt, currentRates, 0.01, motorCommands);
160
161 // Вывод на моторы
162 setMotorOutputs(motorCommands);
163}
🚁 УРОК 2: РЕЖИМЫ ПОЛЕТА И АВТОПИЛОТ (3.5 часа)
2.1 Иерархия режимов полета
АРХИТЕКТУРА РЕЖИМОВ:
1🎯 КЛАССИФИКАЦИЯ РЕЖИМОВ ПО СЛОЖНОСТИ:
2
3БАЗОВЫЕ РЕЖИМЫ (Ручное управление):
4├── ACRO - Прямое управление угловыми скоростями
5├── STABILIZE - Стабилизация углов
6└── ALT_HOLD - Стабилизация + удержание высоты
7
8НАВИГАЦИОННЫЕ РЕЖИМЫ (Полуавтоматические):
9├── LOITER - Удержание позиции GPS
10├── GUIDED - Полет к указанной точке
11└── CIRCLE - Полет по кругу
12
13АВТОНОМНЫЕ РЕЖИМЫ (Полная автоматизация):
14├── AUTO - Полет по заданному маршруту
15├── RTL - Автоматический возврат домой
16├── LAND - Автоматическая посадка
17└── MISSION - Выполнение сложных миссий
18
19АВАРИЙНЫЕ РЕЖИМЫ (Безопасность):
20├── FAILSAFE - Аварийные процедуры
21├── BRAKE - Экстренное торможение
22└── EMERGENCY_LAND - Аварийная посадка
УНИВЕРСАЛЬНАЯ СИСТЕМА РЕЖИМОВ:
1// =====================================
2// ДИСПЕТЧЕР РЕЖИМОВ ПОЛЕТА
3// =====================================
4
5class FlightModeManager {
6public:
7 enum FlightMode {
8 DISARMED = 0,
9 ACRO = 1,
10 STABILIZE = 2,
11 ALT_HOLD = 3,
12 LOITER = 4,
13 GUIDED = 5,
14 AUTO = 6,
15 RTL = 7,
16 LAND = 8,
17 FAILSAFE = 9
18 };
19
20 struct ControlState {
21 // Входы от пилота
22 float rollInput, pitchInput, yawInput, throttleInput;
23
24 // Навигационные команды
25 float targetLat, targetLon, targetAlt;
26 float targetVelX, targetVelY, targetVelZ;
27
28 // Состояние системы
29 bool isArmed;
30 bool gpsHealthy;
31 bool altitudeHealthy;
32 bool radioHealthy;
33
34 // Выходы для контроллера
35 float rollCommand, pitchCommand, yawCommand, throttleCommand;
36 bool autoControlActive;
37 };
38
39private:
40 FlightMode currentMode = DISARMED;
41 FlightMode previousMode = DISARMED;
42 ControlState state;
43
44 // Для режимов с памятью
45 float altHoldTarget = 0;
46 float loiterLat = 0, loiterLon = 0;
47 bool targetSet = false;
48
49 // Таймеры и безопасность
50 unsigned long modeChangeTime = 0;
51 unsigned long lastRadioUpdate = 0;
52
53public:
54 bool setMode(FlightMode newMode) {
55 // Проверка возможности перехода
56 if (!canTransitionTo(newMode)) {
57 Serial.printf("Cannot transition from %s to %s\n",
58 getModeString(currentMode), getModeString(newMode));
59 return false;
60 }
61
62 // Выход из текущего режима
63 exitMode(currentMode);
64
65 // Смена режима
66 previousMode = currentMode;
67 currentMode = newMode;
68 modeChangeTime = millis();
69
70 // Вход в новый режим
71 enterMode(newMode);
72
73 Serial.printf("Mode changed: %s → %s\n",
74 getModeString(previousMode), getModeString(currentMode));
75
76 return true;
77 }
78
79 void update(ControlState& inputState) {
80 state = inputState;
81
82 // Проверка безопасности
83 checkSafetyConditions();
84
85 // Обработка текущего режима
86 switch (currentMode) {
87 case DISARMED:
88 handleDisarmed();
89 break;
90 case ACRO:
91 handleAcro();
92 break;
93 case STABILIZE:
94 handleStabilize();
95 break;
96 case ALT_HOLD:
97 handleAltHold();
98 break;
99 case LOITER:
100 handleLoiter();
101 break;
102 case GUIDED:
103 handleGuided();
104 break;
105 case AUTO:
106 handleAuto();
107 break;
108 case RTL:
109 handleRTL();
110 break;
111 case LAND:
112 handleLand();
113 break;
114 case FAILSAFE:
115 handleFailsafe();
116 break;
117 }
118
119 // Обновление выходного состояния
120 inputState = state;
121 }
122
123 FlightMode getCurrentMode() { return currentMode; }
124 String getModeString() { return getModeString(currentMode); }
125
126private:
127 bool canTransitionTo(FlightMode newMode) {
128 switch (newMode) {
129 case DISARMED:
130 return true; // Всегда можно disarm
131
132 case ACRO:
133 case STABILIZE:
134 return state.isArmed && state.radioHealthy;
135
136 case ALT_HOLD:
137 return state.isArmed && state.radioHealthy && state.altitudeHealthy;
138
139 case LOITER:
140 case GUIDED:
141 case AUTO:
142 case RTL:
143 return state.isArmed && state.radioHealthy &&
144 state.altitudeHealthy && state.gpsHealthy;
145
146 case LAND:
147 return state.isArmed;
148
149 case FAILSAFE:
150 return true; // Failsafe можно всегда
151 }
152 return false;
153 }
154
155 void enterMode(FlightMode mode) {
156 switch (mode) {
157 case ALT_HOLD:
158 altHoldTarget = getCurrentAltitude();
159 targetSet = true;
160 Serial.printf("Altitude hold target set: %.1fm\n", altHoldTarget);
161 break;
162
163 case LOITER:
164 if (state.gpsHealthy) {
165 loiterLat = getCurrentLatitude();
166 loiterLon = getCurrentLongitude();
167 altHoldTarget = getCurrentAltitude();
168 targetSet = true;
169 Serial.printf("Loiter target: %.6f, %.6f, %.1fm\n",
170 loiterLat, loiterLon, altHoldTarget);
171 }
172 break;
173
174 case RTL:
175 state.targetLat = getHomeLat();
176 state.targetLon = getHomeLon();
177 state.targetAlt = getHomeAlt() + 20.0; // +20m безопасная высота
178 targetSet = true;
179 Serial.println("Return to Launch initiated");
180 break;
181 }
182 }
183
184 void exitMode(FlightMode mode) {
185 // Очистка состояния при выходе из режима
186 targetSet = false;
187 state.autoControlActive = false;
188 }
189
190 void handleDisarmed() {
191 // В режиме DISARMED все моторы выключены
192 state.rollCommand = 0;
193 state.pitchCommand = 0;
194 state.yawCommand = 0;
195 state.throttleCommand = 0;
196 state.autoControlActive = false;
197 }
198
199 void handleAcro() {
200 // Прямое управление угловыми скоростями
201 state.rollCommand = state.rollInput * 720.0; // ±720°/s
202 state.pitchCommand = state.pitchInput * 720.0;
203 state.yawCommand = state.yawInput * 360.0; // ±360°/s
204 state.throttleCommand = state.throttleInput;
205 state.autoControlActive = false;
206 }
207
208 void handleStabilize() {
209 // Стабилизация углов
210 state.rollCommand = state.rollInput * 45.0; // ±45° max angle
211 state.pitchCommand = state.pitchInput * 45.0;
212 state.yawCommand = state.yawInput * 180.0; // ±180°/s yaw rate
213 state.throttleCommand = state.throttleInput;
214 state.autoControlActive = false;
215 }
216
217 void handleAltHold() {
218 // Стабилизация + удержание высоты
219 handleStabilize(); // Базовая стабилизация
220
221 // Автоматическое управление высотой
222 if (targetSet) {
223 // Изменение целевой высоты стиком газа
224 if (abs(state.throttleInput - 0.5) > 0.1) {
225 altHoldTarget += (state.throttleInput - 0.5) * 2.0; // 2 м/с max
226 Serial.printf("New altitude target: %.1fm\n", altHoldTarget);
227 }
228
229 state.targetAlt = altHoldTarget;
230 state.autoControlActive = true;
231 }
232 }
233
234 void handleLoiter() {
235 // Удержание позиции GPS + высоты
236 if (targetSet && state.gpsHealthy) {
237 state.targetLat = loiterLat;
238 state.targetLon = loiterLon;
239 state.targetAlt = altHoldTarget;
240
241 // Пилот может сместить target стиками
242 if (abs(state.rollInput) > 0.1 || abs(state.pitchInput) > 0.1) {
243 // Переключаемся в режим "nudge" - смещение позиции
244 float nudgeDistance = 0.00001; // ~1 метр в градусах
245 loiterLat += state.pitchInput * nudgeDistance;
246 loiterLon += state.rollInput * nudgeDistance;
247 }
248
249 state.autoControlActive = true;
250 } else {
251 // Fallback к ALT_HOLD если GPS потерян
252 setMode(ALT_HOLD);
253 }
254 }
255
256 void handleGuided() {
257 // Полет к заданной точке
258 if (targetSet) {
259 // Команды устанавливаются извне (GCS, companion computer)
260 state.autoControlActive = true;
261 }
262 }
263
264 void handleAuto() {
265 // Выполнение автономной миссии
266 // Здесь должна быть интеграция с mission planner
267 state.autoControlActive = true;
268
269 // Простая заглушка - лететь по квадрату
270 executeSimpleMission();
271 }
272
273 void handleRTL() {
274 // Возврат домой
275 if (!targetSet) {
276 enterMode(RTL); // Переустановка target если потерян
277 }
278
279 float distanceToHome = calculateDistance(
280 getCurrentLatitude(), getCurrentLongitude(),
281 state.targetLat, state.targetLon);
282
283 if (distanceToHome > 5.0) {
284 // Лететь домой
285 state.autoControlActive = true;
286 } else {
287 // Достигли дома - переключаемся на посадку
288 setMode(LAND);
289 }
290 }
291
292 void handleLand() {
293 // Автоматическая посадка
294 state.targetVelZ = -1.0; // Снижение 1 м/с
295 state.autoControlActive = true;
296
297 // Проверка касания земли
298 if (getCurrentAltitude() < 0.5 && getCurrentVerticalSpeed() < 0.1) {
299 Serial.println("Landing detected - disarming");
300 setMode(DISARMED);
301 }
302 }
303
304 void handleFailsafe() {
305 // Аварийные процедуры
306 Serial.println("FAILSAFE MODE - Emergency procedures");
307
308 static unsigned long failsafeStartTime = 0;
309 if (failsafeStartTime == 0) {
310 failsafeStartTime = millis();
311 }
312
313 unsigned long failsafeTime = millis() - failsafeStartTime;
314
315 if (failsafeTime < 5000) {
316 // Первые 5 секунд - попытка стабилизации
317 state.rollCommand = 0;
318 state.pitchCommand = 0;
319 state.yawCommand = 0;
320 state.throttleCommand = 0.4; // Низкий газ
321 } else {
322 // После 5 секунд - аварийная посадка
323 setMode(LAND);
324 failsafeStartTime = 0;
325 }
326
327 state.autoControlActive = true;
328 }
329
330 void checkSafetyConditions() {
331 // Проверка потери радиосвязи
332 if (millis() - lastRadioUpdate > 2000) { // 2 секунды без сигнала
333 state.radioHealthy = false;
334 if (currentMode != FAILSAFE && currentMode != DISARMED) {
335 Serial.println("Radio failsafe triggered!");
336 setMode(FAILSAFE);
337 }
338 }
339
340 // Проверка низкого заряда батареи
341 float batteryVoltage = getBatteryVoltage();
342 if (batteryVoltage < 10.5 && state.isArmed) { // Критично для 3S LiPo
343 Serial.println("Low battery failsafe!");
344 setMode(RTL);
345 }
346
347 // Проверка потери GPS в навигационных режимах
348 if (!state.gpsHealthy &&
349 (currentMode == LOITER || currentMode == GUIDED ||
350 currentMode == AUTO || currentMode == RTL)) {
351 Serial.println("GPS failsafe - switching to ALT_HOLD");
352 setMode(ALT_HOLD);
353 }
354 }
355
356 void executeSimpleMission() {
357 // Простая миссия - полет по квадрату
358 static int waypointIndex = 0;
359 static float waypoints[][2] = {
360 {0.0001, 0.0001}, // +10m North, +10m East
361 {0.0001, -0.0001}, // +10m North, -10m East
362 {-0.0001, -0.0001}, // -10m North, -10m East
363 {-0.0001, 0.0001} // -10m North, +10m East
364 };
365
366 float homeLat = getHomeLat();
367 float homeLon = getHomeLon();
368
369 state.targetLat = homeLat + waypoints[waypointIndex][0];
370 state.targetLon = homeLon + waypoints[waypointIndex][1];
371 state.targetAlt = getHomeAlt() + 10.0;
372
373 // Проверка достижения waypoint
374 float distance = calculateDistance(
375 getCurrentLatitude(), getCurrentLongitude(),
376 state.targetLat, state.targetLon);
377
378 if (distance < 2.0) { // Достигли waypoint с точностью 2м
379 waypointIndex = (waypointIndex + 1) % 4;
380 Serial.printf("Waypoint %d reached, proceeding to waypoint %d\n",
381 waypointIndex-1, waypointIndex);
382 }
383 }
384
385 // Вспомогательные функции
386 String getModeString(FlightMode mode) {
387 switch (mode) {
388 case DISARMED: return "DISARMED";
389 case ACRO: return "ACRO";
390 case STABILIZE: return "STABILIZE";
391 case ALT_HOLD: return "ALT_HOLD";
392 case LOITER: return "LOITER";
393 case GUIDED: return "GUIDED";
394 case AUTO: return "AUTO";
395 case RTL: return "RTL";
396 case LAND: return "LAND";
397 case FAILSAFE: return "FAILSAFE";
398 default: return "UNKNOWN";
399 }
400 }
401
402 float calculateDistance(float lat1, float lon1, float lat2, float lon2) {
403 // Упрощенный расчет расстояния (для небольших дистанций)
404 float dlat = lat2 - lat1;
405 float dlon = lon2 - lon1;
406 return sqrt(dlat*dlat + dlon*dlon) * 111319.5; // Примерно в метрах
407 }
408
409 // Заглушки для получения данных датчиков
410 float getCurrentAltitude() { return 10.0; } // Получить из барометра
411 float getCurrentLatitude() { return 0.0; } // Получить из GPS
412 float getCurrentLongitude() { return 0.0; } // Получить из GPS
413 float getCurrentVerticalSpeed() { return 0.0; }
414 float getBatteryVoltage() { return 12.0; }
415 float getHomeLat() { return 0.0; }
416 float getHomeLon() { return 0.0; }
417 float getHomeAlt() { return 0.0; }
418};
419
420// Интеграция в основной цикл
421FlightModeManager flightModes;
422
423void flightLoop() {
424 // Получение входных данных
425 FlightModeManager::ControlState controlState;
426 controlState.rollInput = getRollInput();
427 controlState.pitchInput = getPitchInput();
428 controlState.yawInput = getYawInput();
429 controlState.throttleInput = getThrottleInput();
430
431 controlState.isArmed = getArmStatus();
432 controlState.gpsHealthy = getGPSStatus();
433 controlState.altitudeHealthy = getAltitudeStatus();
434 controlState.radioHealthy = getRadioStatus();
435
436 // Обработка режимов
437 flightModes.update(controlState);
438
439 // Использование результатов
440 if (controlState.autoControlActive) {
441 // Передача в автономный контроллер
442 setAutonomousTarget(controlState.targetLat, controlState.targetLon, controlState.targetAlt);
443 } else {
444 // Передача в ручной контроллер
445 setManualCommands(controlState.rollCommand, controlState.pitchCommand,
446 controlState.yawCommand, controlState.throttleCommand);
447 }
448}
2.2 Системы навигации и планирования миссий
WAYPOINT NAVIGATION:
1// =====================================
2// СИСТЕМА НАВИГАЦИИ ПО WAYPOINTS
3// =====================================
4
5class WaypointNavigator {
6public:
7 struct Waypoint {
8 double latitude, longitude;
9 float altitude;
10 float speed; // Желаемая скорость м/с
11 float acceptanceRadius; // Радиус принятия waypoint
12 int waitTime; // Время ожидания в секундах
13 int action; // Действие в точке (фото, сброс груза и т.д.)
14 };
15
16 struct NavigationState {
17 double currentLat, currentLon;
18 float currentAlt;
19 float currentSpeed;
20 float bearing; // Направление к цели
21 float distance; // Расстояние до цели
22 float crossTrackError; // Отклонение от линии пути
23 };
24
25private:
26 std::vector<Waypoint> mission;
27 int currentWaypointIndex = 0;
28 bool missionActive = false;
29 unsigned long waypointReachTime = 0;
30
31 NavigationState navState;
32
33public:
34 void loadMission(const std::vector<Waypoint>& waypoints) {
35 mission = waypoints;
36 currentWaypointIndex = 0;
37 missionActive = false;
38
39 Serial.printf("Mission loaded: %d waypoints\n", mission.size());
40 for (int i = 0; i < mission.size(); i++) {
41 Serial.printf("WP%d: %.6f, %.6f, %.1fm\n",
42 i, mission[i].latitude, mission[i].longitude, mission[i].altitude);
43 }
44 }
45
46 void startMission() {
47 if (mission.empty()) {
48 Serial.println("No mission loaded!");
49 return;
50 }
51
52 missionActive = true;
53 currentWaypointIndex = 0;
54 waypointReachTime = 0;
55
56 Serial.println("Mission started!");
57 printCurrentWaypoint();
58 }
59
60 void stopMission() {
61 missionActive = false;
62 Serial.println("Mission stopped");
63 }
64
65 bool update(double currentLat, double currentLon, float currentAlt) {
66 if (!missionActive || mission.empty()) {
67 return false;
68 }
69
70 navState.currentLat = currentLat;
71 navState.currentLon = currentLon;
72 navState.currentAlt = currentAlt;
73
74 if (currentWaypointIndex >= mission.size()) {
75 // Миссия завершена
76 missionActive = false;
77 Serial.println("🎉 Mission completed!");
78 return false;
79 }
80
81 Waypoint& currentWP = mission[currentWaypointIndex];
82
83 // Расчет навигационных параметров
84 calculateNavigation(currentWP);
85
86 // Проверка достижения waypoint
87 if (navState.distance <= currentWP.acceptanceRadius) {
88 if (waypointReachTime == 0) {
89 waypointReachTime = millis();
90 Serial.printf("Waypoint %d reached! Distance: %.1fm\n",
91 currentWaypointIndex, navState.distance);
92
93 // Выполнение действия в точке
94 executeWaypointAction(currentWP.action);
95 }
96
97 // Ожидание в точке
98 if (millis() - waypointReachTime >= currentWP.waitTime * 1000) {
99 currentWaypointIndex++;
100 waypointReachTime = 0;
101
102 if (currentWaypointIndex < mission.size()) {
103 Serial.printf("Proceeding to waypoint %d\n", currentWaypointIndex);
104 printCurrentWaypoint();
105 }
106 }
107 }
108
109 return true;
110 }
111
112 NavigationState getNavigationState() { return navState; }
113
114 // Генерация стандартных миссий
115 void generateSurveyMission(double centerLat, double centerLon,
116 float width, float height, float spacing, float altitude) {
117 mission.clear();
118
119 // Сетка для съемки
120 int lines = height / spacing;
121 bool leftToRight = true;
122
123 for (int i = 0; i < lines; i++) {
124 float y = -height/2 + i * spacing;
125
126 if (leftToRight) {
127 // Слева направо
128 addWaypoint(centerLat + y/111319.5, centerLon - width/2/111319.5, altitude);
129 addWaypoint(centerLat + y/111319.5, centerLon + width/2/111319.5, altitude);
130 } else {
131 // Справа налево
132 addWaypoint(centerLat + y/111319.5, centerLon + width/2/111319.5, altitude);
133 addWaypoint(centerLat + y/111319.5, centerLon - width/2/111319.5, altitude);
134 }
135
136 leftToRight = !leftToRight;
137 }
138
139 Serial.printf("Survey mission generated: %d waypoints\n", mission.size());
140 }
141
142 void generateCircleMission(double centerLat, double centerLon,
143 float radius, int points, float altitude) {
144 mission.clear();
145
146 for (int i = 0; i <= points; i++) {
147 float angle = i * 2 * PI / points;
148 float lat = centerLat + (radius * cos(angle)) / 111319.5;
149 float lon = centerLon + (radius * sin(angle)) / 111319.5;
150
151 addWaypoint(lat, lon, altitude);
152 }
153
154 Serial.printf("Circle mission generated: %d waypoints\n", mission.size());
155 }
156
157private:
158 void addWaypoint(double lat, double lon, float alt,
159 float speed = 5.0, float radius = 3.0, int wait = 0, int action = 0) {
160 Waypoint wp;
161 wp.latitude = lat;
162 wp.longitude = lon;
163 wp.altitude = alt;
164 wp.speed = speed;
165 wp.acceptanceRadius = radius;
166 wp.waitTime = wait;
167 wp.action = action;
168
169 mission.push_back(wp);
170 }
171
172 void calculateNavigation(const Waypoint& target) {
173 // Расстояние до цели
174 navState.distance = calculateDistance(
175 navState.currentLat, navState.currentLon,
176 target.latitude, target.longitude);
177
178 // Направление к цели
179 navState.bearing = calculateBearing(
180 navState.currentLat, navState.currentLon,
181 target.latitude, target.longitude);
182
183 // Cross-track error (отклонение от линии пути)
184 if (currentWaypointIndex > 0) {
185 Waypoint& prevWP = mission[currentWaypointIndex - 1];
186 navState.crossTrackError = calculateCrossTrackError(
187 prevWP.latitude, prevWP.longitude,
188 target.latitude, target.longitude,
189 navState.currentLat, navState.currentLon);
190 } else {
191 navState.crossTrackError = 0;
192 }
193 }
194
195 float calculateDistance(double lat1, double lon1, double lat2, double lon2) {
196 // Haversine formula для точного расчета
197 double R = 6371000; // Радиус Земли в метрах
198 double phi1 = lat1 * PI / 180.0;
199 double phi2 = lat2 * PI / 180.0;
200 double deltaPhi = (lat2 - lat1) * PI / 180.0;
201 double deltaLambda = (lon2 - lon1) * PI / 180.0;
202
203 double a = sin(deltaPhi/2) * sin(deltaPhi/2) +
204 cos(phi1) * cos(phi2) *
205 sin(deltaLambda/2) * sin(deltaLambda/2);
206 double c = 2 * atan2(sqrt(a), sqrt(1-a));
207
208 return R * c;
209 }
210
211 float calculateBearing(double lat1, double lon1, double lat2, double lon2) {
212 double phi1 = lat1 * PI / 180.0;
213 double phi2 = lat2 * PI / 180.0;
214 double deltaLambda = (lon2 - lon1) * PI / 180.0;
215
216 double y = sin(deltaLambda) * cos(phi2);
217 double x = cos(phi1) * sin(phi2) - sin(phi1) * cos(phi2) * cos(deltaLambda);
218
219 double bearing = atan2(y, x) * 180.0 / PI;
220 return fmod(bearing + 360.0, 360.0); // Нормализация 0-360°
221 }
222
223 float calculateCrossTrackError(double lat1, double lon1, double lat2, double lon2,
224 double currentLat, double currentLon) {
225 // Упрощенный расчет для небольших расстояний
226 double d13 = calculateDistance(lat1, lon1, currentLat, currentLon);
227 double bearing13 = calculateBearing(lat1, lon1, currentLat, currentLon) * PI / 180.0;
228 double bearing12 = calculateBearing(lat1, lon1, lat2, lon2) * PI / 180.0;
229
230 return asin(sin(d13/6371000) * sin(bearing13 - bearing12)) * 6371000;
231 }
232
233 void executeWaypointAction(int action) {
234 switch (action) {
235 case 1:
236 Serial.println("📸 Taking photo");
237 // triggerCamera();
238 break;
239 case 2:
240 Serial.println("📦 Dropping payload");
241 // dropPayload();
242 break;
243 case 3:
244 Serial.println("📡 Recording telemetry");
245 // recordTelemetry();
246 break;
247 default:
248 // Никакого действия
249 break;
250 }
251 }
252
253 void printCurrentWaypoint() {
254 if (currentWaypointIndex < mission.size()) {
255 Waypoint& wp = mission[currentWaypointIndex];
256 Serial.printf("Current target - WP%d: %.6f, %.6f, %.1fm\n",
257 currentWaypointIndex, wp.latitude, wp.longitude, wp.altitude);
258 }
259 }
260};
261
262// Интеграция с системой управления
263WaypointNavigator navigator;
264
265void missionLoop() {
266 // Получение текущей позиции
267 double currentLat = getCurrentLatitude();
268 double currentLon = getCurrentLongitude();
269 float currentAlt = getCurrentAltitude();
270
271 // Обновление навигации
272 if (navigator.update(currentLat, currentLon, currentAlt)) {
273 WaypointNavigator::NavigationState nav = navigator.getNavigationState();
274
275 // Передача команд в систему управления
276 setNavigationTarget(nav.bearing, nav.distance, nav.crossTrackError);
277
278 // Отладочная информация
279 Serial.printf("Nav: bearing=%.1f° distance=%.1fm crosstrack=%.1fm\n",
280 nav.bearing, nav.distance, nav.crossTrackError);
281 }
282}
📡 УРОК 3: ТЕЛЕМЕТРИЯ И МОНИТОРИНГ (2.5 часа)
3.1 Системы телеметрии реального времени
АРХИТЕКТУРА ТЕЛЕМЕТРИИ:
1📊 МНОГОУРОВНЕВАЯ СИСТЕМА МОНИТОРИНГА:
2
3УРОВЕНЬ 1: СБОР ДАННЫХ (1000Hz)
4├── IMU данные (углы, скорости, ускорения)
5├── Данные моторов (обороты, ток, температура)
6├── Состояние батареи (напряжение, ток, заряд)
7└── Системные метрики (CPU, память, температура)
8
9УРОВЕНЬ 2: ОБРАБОТКА И ФИЛЬТРАЦИЯ (100Hz)
10├── Sensor fusion (объединение датчиков)
11├── State estimation (оценка состояния)
12├── Performance metrics (показатели производительности)
13└── Health monitoring (контроль исправности)
14
15УРОВЕНЬ 3: ПЕРЕДАЧА ДАННЫХ (10Hz)
16├── Critical data (критически важные данные)
17├── Navigation data (навигационная информация)
18├── Status messages (сообщения о состоянии)
19└── Error reports (отчеты об ошибках)
20
21УРОВЕНЬ 4: ВИЗУАЛИЗАЦИЯ (1Hz)
22├── Real-time dashboard (панель в реальном времени)
23├── Historical plots (графики истории)
24├── 3D visualization (3D визуализация)
25└── Alert notifications (уведомления о проблемах)
УНИВЕРСАЛЬНАЯ СИСТЕМА ТЕЛЕМЕТРИИ:
1// =====================================
2// КОМПЛЕКСНАЯ СИСТЕМА ТЕЛЕМЕТРИИ
3// =====================================
4
5#include <WiFi.h>
6#include <WebServer.h>
7#include <WebSocketsServer.h>
8#include <ArduinoJson.h>
9#include <SPIFFS.h>
10
11class TelemetrySystem {
12public:
13 struct FlightData {
14 // Временная метка
15 unsigned long timestamp;
16
17 // Ориентация и движение
18 float roll, pitch, yaw;
19 float rollRate, pitchRate, yawRate;
20 float accelX, accelY, accelZ;
21
22 // Позиция и навигация
23 double latitude, longitude;
24 float altitude, altitudeGPS;
25 float velocityN, velocityE, velocityD;
26 float groundSpeed, airSpeed;
27
28 // Система управления
29 float motor1, motor2, motor3, motor4;
30 float rollCommand, pitchCommand, yawCommand, throttleCommand;
31 int flightMode;
32 bool armed;
33
34 // Система питания
35 float batteryVoltage, batteryCurrent;
36 float batteryCapacityUsed, batteryRemaining;
37 int batteryCells;
38
39 // Датчики окружения
40 float temperature, pressure, humidity;
41 float windSpeed, windDirection;
42
43 // Системные данные
44 float cpuUsage, memoryUsage;
45 int wifiSignal, gpsStatus;
46 int loopFrequency;
47
48 // Статус и ошибки
49 uint32_t statusFlags;
50 uint32_t errorFlags;
51 };
52
53private:
54 WebServer httpServer;
55 WebSocketsServer webSocketServer;
56
57 FlightData currentData;
58 std::vector<FlightData> dataBuffer;
59 static const int BUFFER_SIZE = 1000;
60
61 // Настройки передачи
62 unsigned long lastTelemetryUpdate = 0;
63 unsigned long lastLogWrite = 0;
64 int telemetryRate = 10; // Hz
65 int logRate = 50; // Hz
66
67 bool loggingEnabled = true;
68 File logFile;
69
70public:
71 TelemetrySystem() : httpServer(80), webSocketServer(81) {}
72
73 void begin() {
74 SPIFFS.begin();
75 setupWebServer();
76 setupWebSocket();
77 startLogging();
78
79 httpServer.begin();
80 webSocketServer.begin();
81
82 Serial.println("Telemetry system started");
83 Serial.printf("Web interface: http://%s\n", WiFi.localIP().toString().c_str());
84 }
85
86 void update(const FlightData& data) {
87 currentData = data;
88 currentData.timestamp = millis();
89
90 // Добавление в буфер
91 dataBuffer.push_back(currentData);
92 if (dataBuffer.size() > BUFFER_SIZE) {
93 dataBuffer.erase(dataBuffer.begin());
94 }
95
96 // Обработка веб-сервера
97 httpServer.handleClient();
98 webSocketServer.loop();
99
100 // Отправка телеметрии
101 if (millis() - lastTelemetryUpdate >= 1000 / telemetryRate) {
102 sendTelemetryUpdate();
103 lastTelemetryUpdate = millis();
104 }
105
106 // Запись в лог
107 if (loggingEnabled && millis() - lastLogWrite >= 1000 / logRate) {
108 writeToLog(currentData);
109 lastLogWrite = millis();
110 }
111 }
112
113private:
114 void setupWebServer() {
115 // Главная страница
116 httpServer.on("/", [this]() {
117 httpServer.send(200, "text/html", getMainPage());
118 });
119
120 // API для получения текущих данных
121 httpServer.on("/api/current", [this]() {
122 DynamicJsonDocument doc(2048);
123 serializeFlightData(currentData, doc);
124
125 String json;
126 serializeJson(doc, json);
127 httpServer.send(200, "application/json", json);
128 });
129
130 // API для получения истории
131 httpServer.on("/api/history", [this]() {
132 DynamicJsonDocument doc(8192);
133 JsonArray array = doc.to<JsonArray>();
134
135 int startIndex = max(0, (int)dataBuffer.size() - 100);
136 for (int i = startIndex; i < dataBuffer.size(); i++) {
137 JsonObject obj = array.createNestedObject();
138 serializeFlightData(dataBuffer[i], obj);
139 }
140
141 String json;
142 serializeJson(doc, json);
143 httpServer.send(200, "application/json", json);
144 });
145
146 // Скачивание логов
147 httpServer.on("/api/download_log", [this]() {
148 if (SPIFFS.exists("/flight.log")) {
149 File file = SPIFFS.open("/flight.log", "r");
150 httpServer.streamFile(file, "application/octet-stream");
151 file.close();
152 } else {
153 httpServer.send(404, "text/plain", "Log file not found");
154 }
155 });
156 }
157
158 void setupWebSocket() {
159 webSocketServer.onEvent([this](uint8_t num, WStype_t type, uint8_t* payload, size_t length) {
160 switch (type) {
161 case WStype_DISCONNECTED:
162 Serial.printf("WebSocket client %u disconnected\n", num);
163 break;
164
165 case WStype_CONNECTED:
166 Serial.printf("WebSocket client %u connected\n", num);
167 break;
168
169 case WStype_TEXT:
170 processWebSocketCommand(num, (char*)payload);
171 break;
172 }
173 });
174 }
175
176 void sendTelemetryUpdate() {
177 DynamicJsonDocument doc(1024);
178 serializeFlightData(currentData, doc);
179
180 String json;
181 serializeJson(doc, json);
182
183 webSocketServer.broadcastTXT(json);
184 }
185
186 void serializeFlightData(const FlightData& data, JsonDocument& doc) {
187 doc["timestamp"] = data.timestamp;
188 doc["roll"] = data.roll;
189 doc["pitch"] = data.pitch;
190 doc["yaw"] = data.yaw;
191 doc["altitude"] = data.altitude;
192 doc["latitude"] = data.latitude;
193 doc["longitude"] = data.longitude;
194 doc["battery_voltage"] = data.batteryVoltage;
195 doc["battery_current"] = data.batteryCurrent;
196 doc["flight_mode"] = data.flightMode;
197 doc["armed"] = data.armed;
198 doc["motor1"] = data.motor1;
199 doc["motor2"] = data.motor2;
200 doc["motor3"] = data.motor3;
201 doc["motor4"] = data.motor4;
202 }
203
204 void startLogging() {
205 if (loggingEnabled) {
206 logFile = SPIFFS.open("/flight.log", "w");
207 if (logFile) {
208 // Заголовок CSV
209 logFile.println("timestamp,roll,pitch,yaw,altitude,lat,lon,battery_v,battery_a,mode,armed,m1,m2,m3,m4");
210 Serial.println("Flight logging started");
211 }
212 }
213 }
214
215 void writeToLog(const FlightData& data) {
216 if (logFile && loggingEnabled) {
217 logFile.printf("%lu,%.2f,%.2f,%.2f,%.1f,%.6f,%.6f,%.2f,%.2f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
218 data.timestamp, data.roll, data.pitch, data.yaw, data.altitude,
219 data.latitude, data.longitude, data.batteryVoltage, data.batteryCurrent,
220 data.flightMode, data.armed, data.motor1, data.motor2, data.motor3, data.motor4);
221 logFile.flush();
222 }
223 }
224
225 void processWebSocketCommand(uint8_t clientNum, const char* command) {
226 DynamicJsonDocument doc(512);
227 deserializeJson(doc, command);
228
229 String cmd = doc["command"];
230
231 if (cmd == "set_telemetry_rate") {
232 telemetryRate = doc["rate"];
233 Serial.printf("Telemetry rate set to %d Hz\n", telemetryRate);
234 }
235 else if (cmd == "toggle_logging") {
236 loggingEnabled = !loggingEnabled;
237 Serial.printf("Logging %s\n", loggingEnabled ? "enabled" : "disabled");
238 }
239 else if (cmd == "clear_log") {
240 if (logFile) {
241 logFile.close();
242 SPIFFS.remove("/flight.log");
243 startLogging();
244 Serial.println("Flight log cleared");
245 }
246 }
247 }
248
249 String getMainPage() {
250 return R"HTML(
251<!DOCTYPE html>
252<html>
253<head>
254 <title>Drone Telemetry Dashboard</title>
255 <meta charset="utf-8">
256 <meta name="viewport" content="width=device-width, initial-scale=1">
257 <style>
258 body {
259 font-family: 'Segoe UI', Arial, sans-serif;
260 margin: 0;
261 padding: 20px;
262 background: linear-gradient(135deg, #667eea 0%, #764ba2 100%);
263 color: white;
264 }
265 .container {
266 max-width: 1400px;
267 margin: 0 auto;
268 }
269 .header {
270 text-align: center;
271 margin-bottom: 30px;
272 }
273 .header h1 {
274 font-size: 2.5em;
275 margin: 0;
276 text-shadow: 2px 2px 4px rgba(0,0,0,0.3);
277 }
278 .status-bar {
279 display: flex;
280 justify-content: center;
281 gap: 20px;
282 margin-bottom: 30px;
283 }
284 .status-item {
285 background: rgba(255,255,255,0.2);
286 padding: 10px 20px;
287 border-radius: 20px;
288 backdrop-filter: blur(10px);
289 }
290 .grid {
291 display: grid;
292 grid-template-columns: repeat(auto-fit, minmax(300px, 1fr));
293 gap: 20px;
294 margin-bottom: 30px;
295 }
296 .card {
297 background: rgba(255,255,255,0.15);
298 backdrop-filter: blur(10px);
299 padding: 20px;
300 border-radius: 15px;
301 box-shadow: 0 8px 32px rgba(0,0,0,0.1);
302 border: 1px solid rgba(255,255,255,0.2);
303 }
304 .card h3 {
305 margin-top: 0;
306 font-size: 1.2em;
307 border-bottom: 2px solid rgba(255,255,255,0.3);
308 padding-bottom: 10px;
309 }
310 .metric {
311 display: flex;
312 justify-content: space-between;
313 margin: 10px 0;
314 padding: 8px 0;
315 border-bottom: 1px solid rgba(255,255,255,0.1);
316 }
317 .metric-label {
318 font-weight: 500;
319 }
320 .metric-value {
321 font-weight: bold;
322 font-family: 'Courier New', monospace;
323 }
324 .attitude-indicator {
325 width: 200px;
326 height: 200px;
327 margin: 20px auto;
328 position: relative;
329 border-radius: 50%;
330 background: radial-gradient(circle, #87CEEB 0%, #4682B4 100%);
331 border: 5px solid rgba(255,255,255,0.3);
332 }
333 .horizon {
334 position: absolute;
335 width: 100%;
336 height: 50%;
337 bottom: 0;
338 background: linear-gradient(to bottom, #8B4513 0%, #A0522D 100%);
339 border-radius: 0 0 50% 50%;
340 transform-origin: center top;
341 transition: transform 0.3s ease;
342 }
343 .aircraft-symbol {
344 position: absolute;
345 top: 50%;
346 left: 50%;
347 transform: translate(-50%, -50%);
348 width: 60px;
349 height: 4px;
350 background: #FFD700;
351 border-radius: 2px;
352 }
353 .aircraft-symbol::before {
354 content: '';
355 position: absolute;
356 top: -8px;
357 left: 50%;
358 transform: translateX(-50%);
359 width: 20px;
360 height: 4px;
361 background: #FFD700;
362 border-radius: 2px;
363 }
364 .controls {
365 display: flex;
366 gap: 10px;
367 margin-top: 20px;
368 justify-content: center;
369 }
370 .btn {
371 padding: 10px 20px;
372 background: rgba(255,255,255,0.2);
373 border: none;
374 border-radius: 25px;
375 color: white;
376 cursor: pointer;
377 transition: all 0.3s ease;
378 backdrop-filter: blur(10px);
379 }
380 .btn:hover {
381 background: rgba(255,255,255,0.3);
382 transform: translateY(-2px);
383 }
384 .chart-container {
385 grid-column: 1 / -1;
386 height: 300px;
387 background: rgba(255,255,255,0.1);
388 border-radius: 15px;
389 padding: 20px;
390 }
391 #chart {
392 width: 100%;
393 height: 100%;
394 }
395 .connection-status {
396 position: fixed;
397 top: 20px;
398 right: 20px;
399 padding: 10px 15px;
400 border-radius: 20px;
401 font-weight: bold;
402 transition: all 0.3s ease;
403 }
404 .connected {
405 background: rgba(76, 175, 80, 0.9);
406 }
407 .disconnected {
408 background: rgba(244, 67, 54, 0.9);
409 }
410 </style>
411 <script src="https://cdn.jsdelivr.net/npm/chart.js"></script>
412</head>
413<body>
414 <div id="connectionStatus" class="connection-status disconnected">
415 ⚫ Disconnected
416 </div>
417
418 <div class="container">
419 <div class="header">
420 <h1>🚁 Drone Telemetry Dashboard</h1>
421 </div>
422
423 <div class="status-bar">
424 <div class="status-item">
425 <span id="flightMode">DISARMED</span>
426 </div>
427 <div class="status-item">
428 <span id="armedStatus">DISARMED</span>
429 </div>
430 <div class="status-item">
431 GPS: <span id="gpsStatus">No Fix</span>
432 </div>
433 </div>
434
435 <div class="grid">
436 <div class="card">
437 <h3>🧭 Attitude</h3>
438 <div class="attitude-indicator">
439 <div class="horizon" id="horizon"></div>
440 <div class="aircraft-symbol"></div>
441 </div>
442 <div class="metric">
443 <span class="metric-label">Roll:</span>
444 <span class="metric-value" id="roll">0.0°</span>
445 </div>
446 <div class="metric">
447 <span class="metric-label">Pitch:</span>
448 <span class="metric-value" id="pitch">0.0°</span>
449 </div>
450 <div class="metric">
451 <span class="metric-label">Yaw:</span>
452 <span class="metric-value" id="yaw">0.0°</span>
453 </div>
454 </div>
455
456 <div class="card">
457 <h3>📍 Position</h3>
458 <div class="metric">
459 <span class="metric-label">Latitude:</span>
460 <span class="metric-value" id="latitude">0.000000</span>
461 </div>
462 <div class="metric">
463 <span class="metric-label">Longitude:</span>
464 <span class="metric-value" id="longitude">0.000000</span>
465 </div>
466 <div class="metric">
467 <span class="metric-label">Altitude:</span>
468 <span class="metric-value" id="altitude">0.0 m</span>
469 </div>
470 <div class="metric">
471 <span class="metric-label">Ground Speed:</span>
472 <span class="metric-value" id="groundSpeed">0.0 m/s</span>
473 </div>
474 </div>
475
476 <div class="card">
477 <h3>🔋 Power System</h3>
478 <div class="metric">
479 <span class="metric-label">Battery Voltage:</span>
480 <span class="metric-value" id="batteryVoltage">0.0 V</span>
481 </div>
482 <div class="metric">
483 <span class="metric-label">Current Draw:</span>
484 <span class="metric-value" id="batteryCurrent">0.0 A</span>
485 </div>
486 <div class="metric">
487 <span class="metric-label">Power:</span>
488 <span class="metric-value" id="power">0.0 W</span>
489 </div>
490 <div class="metric">
491 <span class="metric-label">Remaining:</span>
492 <span class="metric-value" id="batteryRemaining">100%</span>
493 </div>
494 </div>
495
496 <div class="card">
497 <h3>🚁 Motors</h3>
498 <div class="metric">
499 <span class="metric-label">Motor 1:</span>
500 <span class="metric-value" id="motor1">0%</span>
501 </div>
502 <div class="metric">
503 <span class="metric-label">Motor 2:</span>
504 <span class="metric-value" id="motor2">0%</span>
505 </div>
506 <div class="metric">
507 <span class="metric-label">Motor 3:</span>
508 <span class="metric-value" id="motor3">0%</span>
509 </div>
510 <div class="metric">
511 <span class="metric-label">Motor 4:</span>
512 <span class="metric-value" id="motor4">0%</span>
513 </div>
514 </div>
515
516 <div class="card chart-container">
517 <h3>📊 Real-time Data</h3>
518 <canvas id="chart"></canvas>
519 </div>
520 </div>
521
522 <div class="controls">
523 <button class="btn" onclick="toggleLogging()">📝 Toggle Logging</button>
524 <button class="btn" onclick="downloadLogs()">📥 Download Logs</button>
525 <button class="btn" onclick="clearLogs()">🗑️ Clear Logs</button>
526 <button class="btn" onclick="changeTelemetryRate()">⚡ Change Rate</button>
527 </div>
528 </div>
529
530 <script>
531 let ws;
532 let chart;
533 let dataPoints = [];
534 const maxDataPoints = 50;
535
536 function initWebSocket() {
537 const wsUrl = `ws://${window.location.hostname}:81`;
538 ws = new WebSocket(wsUrl);
539
540 ws.onopen = function() {
541 document.getElementById('connectionStatus').className = 'connection-status connected';
542 document.getElementById('connectionStatus').innerHTML = '🟢 Connected';
543 console.log('WebSocket connected');
544 };
545
546 ws.onclose = function() {
547 document.getElementById('connectionStatus').className = 'connection-status disconnected';
548 document.getElementById('connectionStatus').innerHTML = '🔴 Disconnected';
549 console.log('WebSocket disconnected, attempting reconnect...');
550 setTimeout(initWebSocket, 2000);
551 };
552
553 ws.onmessage = function(event) {
554 try {
555 const data = JSON.parse(event.data);
556 updateDashboard(data);
557 updateChart(data);
558 } catch (e) {
559 console.error('Error parsing telemetry data:', e);
560 }
561 };
562
563 ws.onerror = function(error) {
564 console.error('WebSocket error:', error);
565 };
566 }
567
568 function updateDashboard(data) {
569 // Attitude
570 document.getElementById('roll').textContent = data.roll.toFixed(1) + '°';
571 document.getElementById('pitch').textContent = data.pitch.toFixed(1) + '°';
572 document.getElementById('yaw').textContent = data.yaw.toFixed(1) + '°';
573
574 // Update attitude indicator
575 const horizon = document.getElementById('horizon');
576 horizon.style.transform = `rotate(${-data.roll}deg) translateY(${data.pitch}px)`;
577
578 // Position
579 document.getElementById('latitude').textContent = data.latitude.toFixed(6);
580 document.getElementById('longitude').textContent = data.longitude.toFixed(6);
581 document.getElementById('altitude').textContent = data.altitude.toFixed(1) + ' m';
582
583 // Power
584 document.getElementById('batteryVoltage').textContent = data.battery_voltage.toFixed(2) + ' V';
585 document.getElementById('batteryCurrent').textContent = data.battery_current.toFixed(1) + ' A';
586 document.getElementById('power').textContent = (data.battery_voltage * data.battery_current).toFixed(1) + ' W';
587
588 // Motors
589 document.getElementById('motor1').textContent = (data.motor1 * 100).toFixed(0) + '%';
590 document.getElementById('motor2').textContent = (data.motor2 * 100).toFixed(0) + '%';
591 document.getElementById('motor3').textContent = (data.motor3 * 100).toFixed(0) + '%';
592 document.getElementById('motor4').textContent = (data.motor4 * 100).toFixed(0) + '%';
593
594 // Status
595 const modes = ['DISARMED', 'ACRO', 'STABILIZE', 'ALT_HOLD', 'LOITER', 'GUIDED', 'AUTO', 'RTL', 'LAND', 'FAILSAFE'];
596 document.getElementById('flightMode').textContent = modes[data.flight_mode] || 'UNKNOWN';
597 document.getElementById('armedStatus').textContent = data.armed ? 'ARMED' : 'DISARMED';
598 document.getElementById('armedStatus').style.color = data.armed ? '#ff4444' : '#44ff44';
599 }
600
601 function initChart() {
602 const ctx = document.getElementById('chart').getContext('2d');
603 chart = new Chart(ctx, {
604 type: 'line',
605 data: {
606 labels: [],
607 datasets: [
608 {
609 label: 'Roll (°)',
610 data: [],
611 borderColor: '#ff6384',
612 backgroundColor: 'rgba(255, 99, 132, 0.1)',
613 tension: 0.1,
614 fill: false
615 },
616 {
617 label: 'Pitch (°)',
618 data: [],
619 borderColor: '#36a2eb',
620 backgroundColor: 'rgba(54, 162, 235, 0.1)',
621 tension: 0.1,
622 fill: false
623 },
624 {
625 label: 'Altitude (m)',
626 data: [],
627 borderColor: '#4bc0c0',
628 backgroundColor: 'rgba(75, 192, 192, 0.1)',
629 tension: 0.1,
630 fill: false,
631 yAxisID: 'y1'
632 }
633 ]
634 },
635 options: {
636 responsive: true,
637 maintainAspectRatio: false,
638 plugins: {
639 legend: {
640 labels: {
641 color: 'white'
642 }
643 }
644 },
645 scales: {
646 x: {
647 ticks: { color: 'white' },
648 grid: { color: 'rgba(255,255,255,0.1)' }
649 },
650 y: {
651 type: 'linear',
652 display: true,
653 position: 'left',
654 ticks: { color: 'white' },
655 grid: { color: 'rgba(255,255,255,0.1)' }
656 },
657 y1: {
658 type: 'linear',
659 display: true,
660 position: 'right',
661 ticks: { color: 'white' },
662 grid: { drawOnChartArea: false }
663 }
664 }
665 }
666 });
667 }
668
669 function updateChart(data) {
670 const time = new Date().toLocaleTimeString();
671
672 chart.data.labels.push(time);
673 chart.data.datasets[0].data.push(data.roll);
674 chart.data.datasets[1].data.push(data.pitch);
675 chart.data.datasets[2].data.push(data.altitude);
676
677 if (chart.data.labels.length > maxDataPoints) {
678 chart.data.labels.shift();
679 chart.data.datasets.forEach(dataset => dataset.data.shift());
680 }
681
682 chart.update('none');
683 }
684
685 function sendCommand(command, params = {}) {
686 if (ws && ws.readyState === WebSocket.OPEN) {
687 const message = { command, ...params };
688 ws.send(JSON.stringify(message));
689 }
690 }
691
692 function toggleLogging() {
693 sendCommand('toggle_logging');
694 }
695
696 function downloadLogs() {
697 window.open('/api/download_log');
698 }
699
700 function clearLogs() {
701 if (confirm('Clear all flight logs? This cannot be undone.')) {
702 sendCommand('clear_log');
703 }
704 }
705
706 function changeTelemetryRate() {
707 const rate = prompt('Enter telemetry rate (1-100 Hz):', '10');
708 if (rate && !isNaN(rate)) {
709 sendCommand('set_telemetry_rate', { rate: parseInt(rate) });
710 }
711 }
712
713 // Initialize everything
714 document.addEventListener('DOMContentLoaded', function() {
715 initChart();
716 initWebSocket();
717 });
718 </script>
719</body>
720</html>
721 )HTML";
722 }
723};
724
725// Использование системы телеметрии
726TelemetrySystem telemetry;
727
728void setup() {
729 Serial.begin(115200);
730 WiFi.begin("YourWiFi", "password");
731
732 while (WiFi.status() != WL_CONNECTED) {
733 delay(1000);
734 Serial.println("Connecting to WiFi...");
735 }
736
737 telemetry.begin();
738}
739
740void loop() {
741 // Сбор данных полета
742 TelemetrySystem::FlightData flightData;
743
744 // Заполнение данных из датчиков
745 flightData.roll = getCurrentRoll();
746 flightData.pitch = getCurrentPitch();
747 flightData.yaw = getCurrentYaw();
748 flightData.altitude = getCurrentAltitude();
749 flightData.latitude = getCurrentLatitude();
750 flightData.longitude = getCurrentLongitude();
751 flightData.batteryVoltage = getBatteryVoltage();
752 flightData.batteryCurrent = getBatteryCurrent();
753 flightData.flightMode = getCurrentFlightMode();
754 flightData.armed = isArmed();
755
756 // Обновление телеметрии
757 telemetry.update(flightData);
758
759 delay(20); // 50Hz основной цикл
760}
3.2 Продвинутая диагностика системы
СИСТЕМА МОНИТОРИНГА ЗДОРОВЬЯ:
1// =====================================
2// ДИАГНОСТИКА И МОНИТОРИНГ ЗДОРОВЬЯ СИСТЕМЫ
3// =====================================
4
5class SystemHealthMonitor {
6public:
7 enum HealthStatus {
8 HEALTHY = 0,
9 WARNING = 1,
10 CRITICAL = 2,
11 FAILURE = 3
12 };
13
14 struct ComponentHealth {
15 String componentName;
16 HealthStatus status;
17 float healthScore; // 0.0 - 1.0
18 String statusMessage;
19 unsigned long lastUpdate;
20 std::vector<String> symptoms;
21 };
22
23 struct SystemMetrics {
24 float overallHealth;
25 int healthyComponents;
26 int warningComponents;
27 int criticalComponents;
28 int failedComponents;
29
30 // Performance metrics
31 float averageLoopTime;
32 float cpuUsage;
33 float memoryUsage;
34 int droppedPackets;
35
36 // Flight metrics
37 float flightTime;
38 float totalDistance;
39 int landingCount;
40 float averageBatteryLife;
41 };
42
43private:
44 std::map<String, ComponentHealth> components;
45 SystemMetrics metrics;
46
47 // Thresholds for health scoring
48 struct HealthThresholds {
49 float batteryVoltageMin = 10.5; // 3S LiPo critical
50 float batteryVoltageWarning = 11.1; // 3S LiPo warning
51 float temperatureMax = 85.0; // °C critical
52 float temperatureWarning = 75.0; // °C warning
53 float vibrationMax = 5.0; // m/s² critical
54 float vibrationWarning = 3.0; // m/s² warning
55 float gpsHdopMax = 5.0; // HDOP critical
56 float gpsHdopWarning = 2.0; // HDOP warning
57 } thresholds;
58
59 unsigned long lastHealthCheck = 0;
60 const unsigned long healthCheckInterval = 1000; // 1 second
61
62public:
63 void begin() {
64 // Initialize component health tracking
65 registerComponent("IMU", "Inertial Measurement Unit");
66 registerComponent("GPS", "Global Positioning System");
67 registerComponent("BARO", "Barometric Altimeter");
68 registerComponent("BATTERY", "Power System");
69 registerComponent("MOTORS", "Motor Control System");
70 registerComponent("RADIO", "Radio Communication");
71 registerComponent("STORAGE", "Data Storage System");
72 registerComponent("THERMAL", "Thermal Management");
73
74 Serial.println("System Health Monitor initialized");
75 }
76
77 void update() {
78 if (millis() - lastHealthCheck >= healthCheckInterval) {
79 checkAllComponents();
80 calculateOverallHealth();
81 updateMetrics();
82 checkCriticalConditions();
83 lastHealthCheck = millis();
84 }
85 }
86
87 SystemMetrics getMetrics() { return metrics; }
88
89 ComponentHealth getComponentHealth(const String& componentName) {
90 return components[componentName];
91 }
92
93 void printHealthReport() {
94 Serial.println("\n=== SYSTEM HEALTH REPORT ===");
95 Serial.printf("Overall Health: %.1f%% (%s)\n",
96 metrics.overallHealth * 100, getHealthStatusString(getOverallStatus()).c_str());
97
98 Serial.printf("Components: %d healthy, %d warning, %d critical, %d failed\n",
99 metrics.healthyComponents, metrics.warningComponents,
100 metrics.criticalComponents, metrics.failedComponents);
101
102 Serial.println("\nComponent Details:");
103 for (const auto& pair : components) {
104 const ComponentHealth& health = pair.second;
105 Serial.printf("%-10s: %s (%.1f%%) - %s\n",
106 health.componentName.c_str(),
107 getHealthStatusString(health.status).c_str(),
108 health.healthScore * 100,
109 health.statusMessage.c_str());
110 }
111
112 if (metrics.criticalComponents > 0 || metrics.failedComponents > 0) {
113 Serial.println("\n⚠️ CRITICAL ISSUES DETECTED:");
114 for (const auto& pair : components) {
115 const ComponentHealth& health = pair.second;
116 if (health.status >= CRITICAL) {
117 Serial.printf("- %s: %s\n", health.componentName.c_str(), health.statusMessage.c_str());
118 for (const String& symptom : health.symptoms) {
119 Serial.printf(" • %s\n", symptom.c_str());
120 }
121 }
122 }
123 }
124 }
125
126 bool isFlightSafe() {
127 return metrics.criticalComponents == 0 && metrics.failedComponents == 0;
128 }
129
130private:
131 void registerComponent(const String& id, const String& name) {
132 ComponentHealth health;
133 health.componentName = name;
134 health.status = HEALTHY;
135 health.healthScore = 1.0;
136 health.statusMessage = "Initialized";
137 health.lastUpdate = millis();
138
139 components[id] = health;
140 }
141
142 void checkAllComponents() {
143 checkIMUHealth();
144 checkGPSHealth();
145 checkBarometerHealth();
146 checkBatteryHealth();
147 checkMotorHealth();
148 checkRadioHealth();
149 checkStorageHealth();
150 checkThermalHealth();
151 }
152
153 void checkIMUHealth() {
154 ComponentHealth& health = components["IMU"];
155 health.symptoms.clear();
156
157 // Simulate IMU health checks
158 float vibrationLevel = getVibrationLevel();
159 float temperature = getIMUTemperature();
160 float dataRate = getIMUDataRate();
161
162 float score = 1.0;
163 HealthStatus status = HEALTHY;
164 String message = "Operating normally";
165
166 // Check vibration levels
167 if (vibrationLevel > thresholds.vibrationMax) {
168 status = CRITICAL;
169 score *= 0.3;
170 health.symptoms.push_back("Excessive vibration detected");
171 message = "High vibration levels";
172 } else if (vibrationLevel > thresholds.vibrationWarning) {
173 status = max(status, WARNING);
174 score *= 0.7;
175 health.symptoms.push_back("Elevated vibration levels");
176 message = "Moderate vibration";
177 }
178
179 // Check temperature
180 if (temperature > thresholds.temperatureMax) {
181 status = max(status, CRITICAL);
182 score *= 0.4;
183 health.symptoms.push_back("IMU overheating");
184 } else if (temperature > thresholds.temperatureWarning) {
185 status = max(status, WARNING);
186 score *= 0.8;
187 health.symptoms.push_back("IMU running hot");
188 }
189
190 // Check data rate
191 if (dataRate < 800) { // Expected 1000Hz
192 status = max(status, WARNING);
193 score *= 0.6;
194 health.symptoms.push_back("Reduced data rate");
195 }
196
197 health.status = status;
198 health.healthScore = score;
199 health.statusMessage = message;
200 health.lastUpdate = millis();
201 }
202
203 void checkGPSHealth() {
204 ComponentHealth& health = components["GPS"];
205 health.symptoms.clear();
206
207 int satellites = getGPSSatellites();
208 float hdop = getGPSHDOP();
209 bool fix3D = hasGPS3DFix();
210
211 float score = 1.0;
212 HealthStatus status = HEALTHY;
213 String message = "GPS fix acquired";
214
215 if (!fix3D) {
216 status = CRITICAL;
217 score = 0.2;
218 health.symptoms.push_back("No 3D GPS fix");
219 message = "No GPS fix";
220 } else {
221 if (satellites < 6) {
222 status = max(status, WARNING);
223 score *= 0.7;
224 health.symptoms.push_back("Low satellite count");
225 message = "Few satellites";
226 }
227
228 if (hdop > thresholds.gpsHdopMax) {
229 status = max(status, CRITICAL);
230 score *= 0.4;
231 health.symptoms.push_back("Poor GPS accuracy");
232 message = "Poor GPS accuracy";
233 } else if (hdop > thresholds.gpsHdopWarning) {
234 status = max(status, WARNING);
235 score *= 0.8;
236 health.symptoms.push_back("Reduced GPS accuracy");
237 }
238 }
239
240 health.status = status;
241 health.healthScore = score;
242 health.statusMessage = message;
243 health.lastUpdate = millis();
244 }
245
246 void checkBatteryHealth() {
247 ComponentHealth& health = components["BATTERY"];
248 health.symptoms.clear();
249
250 float voltage = getBatteryVoltage();
251 float current = getBatteryCurrent();
252 float temperature = getBatteryTemperature();
253 float capacity = getRemainingCapacity();
254
255 float score = 1.0;
256 HealthStatus status = HEALTHY;
257 String message = "Battery healthy";
258
259 // Voltage check
260 if (voltage < thresholds.batteryVoltageMin) {
261 status = CRITICAL;
262 score *= 0.2;
263 health.symptoms.push_back("Critical low voltage");
264 message = "Low battery - land immediately";
265 } else if (voltage < thresholds.batteryVoltageWarning) {
266 status = max(status, WARNING);
267 score *= 0.6;
268 health.symptoms.push_back("Low voltage warning");
269 message = "Battery getting low";
270 }
271
272 // Current check (overcurrent protection)
273 if (current > 60.0) { // Amp limit
274 status = max(status, CRITICAL);
275 score *= 0.3;
276 health.symptoms.push_back("Overcurrent detected");
277 }
278
279 // Temperature check
280 if (temperature > 60.0) { // °C
281 status = max(status, WARNING);
282 score *= 0.7;
283 health.symptoms.push_back("Battery overheating");
284 }
285
286 // Capacity check
287 if (capacity < 0.2) { // 20%
288 status = max(status, WARNING);
289 score *= 0.5;
290 health.symptoms.push_back("Low remaining capacity");
291 }
292
293 health.status = status;
294 health.healthScore = score;
295 health.statusMessage = message;
296 health.lastUpdate = millis();
297 }
298
299 void checkMotorHealth() {
300 ComponentHealth& health = components["MOTORS"];
301 health.symptoms.clear();
302
303 float motor1Temp = getMotorTemperature(1);
304 float motor2Temp = getMotorTemperature(2);
305 float motor3Temp = getMotorTemperature(3);
306 float motor4Temp = getMotorTemperature(4);
307
308 float maxTemp = max({motor1Temp, motor2Temp, motor3Temp, motor4Temp});
309 float avgTemp = (motor1Temp + motor2Temp + motor3Temp + motor4Temp) / 4.0;
310
311 // Check for current imbalance
312 float motorImbalance = getMotorCurrentImbalance();
313
314 float score = 1.0;
315 HealthStatus status = HEALTHY;
316 String message = "Motors operating normally";
317
318 if (maxTemp > 100.0) { // °C critical
319 status = CRITICAL;
320 score *= 0.3;
321 health.symptoms.push_back("Motor overheating");
322 message = "Motor overheating detected";
323 } else if (maxTemp > 85.0) { // °C warning
324 status = max(status, WARNING);
325 score *= 0.7;
326 health.symptoms.push_back("Motors running hot");
327 message = "Motors running hot";
328 }
329
330 if (motorImbalance > 0.3) { // 30% imbalance
331 status = max(status, WARNING);
332 score *= 0.6;
333 health.symptoms.push_back("Motor performance imbalance");
334 }
335
336 health.status = status;
337 health.healthScore = score;
338 health.statusMessage = message;
339 health.lastUpdate = millis();
340 }
341
342 void checkRadioHealth() {
343 ComponentHealth& health = components["RADIO"];
344 health.symptoms.clear();
345
346 int rssi = getRadioRSSI();
347 int packetLoss = getPacketLossRate();
348 unsigned long lastPacket = getLastPacketTime();
349
350 float score = 1.0;
351 HealthStatus status = HEALTHY;
352 String message = "Radio link good";
353
354 // Signal strength check
355 if (rssi < -90) { // dBm
356 status = CRITICAL;
357 score *= 0.4;
358 health.symptoms.push_back("Very weak signal");
359 message = "Weak radio signal";
360 } else if (rssi < -80) {
361 status = max(status, WARNING);
362 score *= 0.7;
363 health.symptoms.push_back("Weak signal");
364 }
365
366 // Packet loss check
367 if (packetLoss > 10) { // 10%
368 status = max(status, WARNING);
369 score *= 0.6;
370 health.symptoms.push_back("High packet loss");
371 }
372
373 // Connection timeout check
374 if (millis() - lastPacket > 2000) { // 2 seconds
375 status = FAILURE;
376 score = 0.0;
377 health.symptoms.push_back("Radio connection lost");
378 message = "No radio contact";
379 }
380
381 health.status = status;
382 health.healthScore = score;
383 health.statusMessage = message;
384 health.lastUpdate = millis();
385 }
386
387 void checkStorageHealth() {
388 ComponentHealth& health = components["STORAGE"];
389 health.symptoms.clear();
390
391 float freeSpace = getStorageFreeSpace(); // Percentage
392 int writeErrors = getStorageWriteErrors();
393
394 float score = 1.0;
395 HealthStatus status = HEALTHY;
396 String message = "Storage healthy";
397
398 if (freeSpace < 10.0) { // 10%
399 status = WARNING;
400 score *= 0.7;
401 health.symptoms.push_back("Low storage space");
402 message = "Storage almost full";
403 }
404
405 if (writeErrors > 5) {
406 status = max(status, WARNING);
407 score *= 0.6;
408 health.symptoms.push_back("Storage write errors");
409 }
410
411 health.status = status;
412 health.healthScore = score;
413 health.statusMessage = message;
414 health.lastUpdate = millis();
415 }
416
417 void checkThermalHealth() {
418 ComponentHealth& health = components["THERMAL"];
419 health.symptoms.clear();
420
421 float cpuTemp = getCPUTemperature();
422 float ambientTemp = getAmbientTemperature();
423
424 float score = 1.0;
425 HealthStatus status = HEALTHY;
426 String message = "Temperature normal";
427
428 if (cpuTemp > 85.0) { // °C
429 status = CRITICAL;
430 score *= 0.3;
431 health.symptoms.push_back("CPU overheating");
432 message = "System overheating";
433 } else if (cpuTemp > 75.0) {
434 status = max(status, WARNING);
435 score *= 0.7;
436 health.symptoms.push_back("Elevated CPU temperature");
437 message = "System running hot";
438 }
439
440 health.status = status;
441 health.healthScore = score;
442 health.statusMessage = message;
443 health.lastUpdate = millis();
444 }
445
446 void calculateOverallHealth() {
447 float totalScore = 0;
448 int componentCount = 0;
449
450 metrics.healthyComponents = 0;
451 metrics.warningComponents = 0;
452 metrics.criticalComponents = 0;
453 metrics.failedComponents = 0;
454
455 for (const auto& pair : components) {
456 const ComponentHealth& health = pair.second;
457 totalScore += health.healthScore;
458 componentCount++;
459
460 switch (health.status) {
461 case HEALTHY: metrics.healthyComponents++; break;
462 case WARNING: metrics.warningComponents++; break;
463 case CRITICAL: metrics.criticalComponents++; break;
464 case FAILURE: metrics.failedComponents++; break;
465 }
466 }
467
468 metrics.overallHealth = componentCount > 0 ? totalScore / componentCount : 0;
469 }
470
471 void updateMetrics() {
472 metrics.averageLoopTime = getAverageLoopTime();
473 metrics.cpuUsage = getCPUUsage();
474 metrics.memoryUsage = getMemoryUsage();
475 metrics.droppedPackets = getDroppedPacketCount();
476 }
477
478 void checkCriticalConditions() {
479 if (metrics.failedComponents > 0 || metrics.criticalComponents > 1) {
480 Serial.println("🚨 CRITICAL SYSTEM CONDITION DETECTED!");
481 Serial.println("Recommend immediate landing!");
482
483 // Trigger emergency procedures
484 triggerEmergencyMode();
485 }
486 }
487
488 HealthStatus getOverallStatus() {
489 if (metrics.failedComponents > 0) return FAILURE;
490 if (metrics.criticalComponents > 0) return CRITICAL;
491 if (metrics.warningComponents > 0) return WARNING;
492 return HEALTHY;
493 }
494
495 String getHealthStatusString(HealthStatus status) {
496 switch (status) {
497 case HEALTHY: return "HEALTHY";
498 case WARNING: return "WARNING";
499 case CRITICAL: return "CRITICAL";
500 case FAILURE: return "FAILURE";
501 default: return "UNKNOWN";
502 }
503 }
504
505 // Mock functions for sensor data (replace with real implementations)
506 float getVibrationLevel() { return random(0, 300) / 100.0; }
507 float getIMUTemperature() { return random(2000, 4000) / 100.0; }
508 float getIMUDataRate() { return random(950, 1000); }
509 int getGPSSatellites() { return random(4, 12); }
510 float getGPSHDOP() { return random(100, 500) / 100.0; }
511 bool hasGPS3DFix() { return random(0, 100) > 20; }
512 float getBatteryVoltage() { return random(1050, 1260) / 100.0; }
513 float getBatteryCurrent() { return random(500, 3000) / 100.0; }
514 float getBatteryTemperature() { return random(2000, 4500) / 100.0; }
515 float getRemainingCapacity() { return random(20, 100) / 100.0; }
516 float getMotorTemperature(int motor) { return random(3000, 7000) / 100.0; }
517 float getMotorCurrentImbalance() { return random(0, 50) / 100.0; }
518 int getRadioRSSI() { return random(-100, -40); }
519 int getPacketLossRate() { return random(0, 20); }
520 unsigned long getLastPacketTime() { return millis() - random(0, 3000); }
521 float getStorageFreeSpace() { return random(1000, 9000) / 100.0; }
522 int getStorageWriteErrors() { return random(0, 10); }
523 float getCPUTemperature() { return random(4000, 8000) / 100.0; }
524 float getAmbientTemperature() { return random(1500, 3500) / 100.0; }
525 float getAverageLoopTime() { return random(18, 22) / 1000.0; }
526 float getCPUUsage() { return random(30, 80) / 100.0; }
527 float getMemoryUsage() { return random(40, 90) / 100.0; }
528 int getDroppedPacketCount() { return random(0, 5); }
529
530 void triggerEmergencyMode() {
531 // Implementation for emergency mode
532 Serial.println("Emergency mode activated!");
533 }
534};
535
536// Использование системы мониторинга
537SystemHealthMonitor healthMonitor;
538
539void setup() {
540 Serial.begin(115200);
541 healthMonitor.begin();
542}
543
544void loop() {
545 healthMonitor.update();
546
547 // Периодический вывод отчета (каждые 10 секунд)
548 static unsigned long lastReport = 0;
549 if (millis() - lastReport > 10000) {
550 healthMonitor.printHealthReport();
551 lastReport = millis();
552 }
553
554 delay(100);
555}
3.3 MAVLink протокол и Ground Control Station
ИНТЕГРАЦИЯ С НАЗЕМНЫМИ СТАНЦИЯМИ:
1// =====================================
2// MAVLINK ТЕЛЕМЕТРИЯ
3// =====================================
4
5#include <common/mavlink.h>
6
7class MAVLinkTelemetry {
8private:
9 HardwareSerial* mavSerial;
10 uint8_t systemId = 1;
11 uint8_t componentId = MAV_COMP_ID_AUTOPILOT1;
12 uint8_t targetSystem = 255;
13 uint8_t targetComponent = 0;
14
15 // Heartbeat timing
16 unsigned long lastHeartbeat = 0;
17 const unsigned long heartbeatInterval = 1000; // 1Hz
18
19 // Telemetry timing
20 unsigned long lastAttitude = 0;
21 unsigned long lastGPS = 0;
22 unsigned long lastSysStatus = 0;
23 const unsigned long attitudeRate = 100; // 10Hz
24 const unsigned long gpsRate = 200; // 5Hz
25 const unsigned long sysStatusRate = 1000; // 1Hz
26
27 // Mission management
28 std::vector<mavlink_mission_item_t> currentMission;
29 int currentWaypointIndex = 0;
30 bool missionActive = false;
31
32public:
33 MAVLinkTelemetry(HardwareSerial* serial) : mavSerial(serial) {}
34
35 void begin() {
36 mavSerial->begin(57600);
37 Serial.println("MAVLink telemetry started on 57600 baud");
38 Serial.println("Compatible with Mission Planner, QGroundControl");
39 }
40
41 void update() {
42 // Send heartbeat
43 if (millis() - lastHeartbeat >= heartbeatInterval) {
44 sendHeartbeat();
45 lastHeartbeat = millis();
46 }
47
48 // Send attitude data
49 if (millis() - lastAttitude >= attitudeRate) {
50 sendAttitude();
51 sendRCChannels();
52 lastAttitude = millis();
53 }
54
55 // Send GPS data
56 if (millis() - lastGPS >= gpsRate) {
57 sendGPS();
58 sendGlobalPosition();
59 lastGPS = millis();
60 }
61
62 // Send system status
63 if (millis() - lastSysStatus >= sysStatusRate) {
64 sendSystemStatus();
65 sendBatteryStatus();
66 sendVFRHUD();
67 lastSysStatus = millis();
68 }
69
70 // Process incoming messages
71 processIncomingMessages();
72
73 // Send mission status if active
74 if (missionActive) {
75 sendMissionCurrent();
76 }
77 }
78
79private:
80 void sendHeartbeat() {
81 mavlink_message_t msg;
82 mavlink_heartbeat_t heartbeat;
83
84 heartbeat.type = MAV_TYPE_QUADROTOR;
85 heartbeat.autopilot = MAV_AUTOPILOT_GENERIC;
86
87 // Set base mode based on current state
88 heartbeat.base_mode = 0;
89 if (isArmed()) heartbeat.base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
90 if (hasManualInput()) heartbeat.base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
91 if (isGuidedMode()) heartbeat.base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
92 if (isAutoMode()) heartbeat.base_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
93
94 heartbeat.custom_mode = getCurrentFlightMode();
95 heartbeat.system_status = getSystemStatus();
96 heartbeat.mavlink_version = 3;
97
98 mavlink_msg_heartbeat_encode(systemId, componentId, &msg, &heartbeat);
99 sendMessage(msg);
100 }
101
102 void sendAttitude() {
103 mavlink_message_t msg;
104 mavlink_attitude_t attitude;
105
106 attitude.time_boot_ms = millis();
107 attitude.roll = getCurrentRoll() * DEG_TO_RAD;
108 attitude.pitch = getCurrentPitch() * DEG_TO_RAD;
109 attitude.yaw = getCurrentYaw() * DEG_TO_RAD;
110 attitude.rollspeed = getCurrentRollRate() * DEG_TO_RAD;
111 attitude.pitchspeed = getCurrentPitchRate() * DEG_TO_RAD;
112 attitude.yawspeed = getCurrentYawRate() * DEG_TO_RAD;
113
114 mavlink_msg_attitude_encode(systemId, componentId, &msg, &attitude);
115 sendMessage(msg);
116 }
117
118 void sendGPS() {
119 mavlink_message_t msg;
120 mavlink_gps_raw_int_t gps;
121
122 gps.time_usec = millis() * 1000;
123 gps.fix_type = getGPSFixType();
124 gps.lat = getCurrentLatitude() * 1e7; // Convert to 1e7 degrees
125 gps.lon = getCurrentLongitude() * 1e7;
126 gps.alt = getGPSAltitude() * 1000; // Convert to mm
127 gps.eph = getGPSHDOP() * 100; // Convert to cm
128 gps.epv = getGPSVDOP() * 100; // Convert to cm
129 gps.vel = getGroundSpeed() * 100; // Convert to cm/s
130 gps.cog = getCourseOverGround() * 100; // Convert to cdeg
131 gps.satellites_visible = getGPSSatellites();
132
133 mavlink_msg_gps_raw_int_encode(systemId, componentId, &msg, &gps);
134 sendMessage(msg);
135 }
136
137 void sendGlobalPosition() {
138 mavlink_message_t msg;
139 mavlink_global_position_int_t pos;
140
141 pos.time_boot_ms = millis();
142 pos.lat = getCurrentLatitude() * 1e7;
143 pos.lon = getCurrentLongitude() * 1e7;
144 pos.alt = getCurrentAltitude() * 1000; // AMSL altitude in mm
145 pos.relative_alt = getRelativeAltitude() * 1000; // Relative altitude in mm
146 pos.vx = getVelocityX() * 100; // cm/s
147 pos.vy = getVelocityY() * 100; // cm/s
148 pos.vz = getVelocityZ() * 100; // cm/s
149 pos.hdg = getCurrentYaw() * 100; // cdeg
150
151 mavlink_msg_global_position_int_encode(systemId, componentId, &msg, &pos);
152 sendMessage(msg);
153 }
154
155 void sendSystemStatus() {
156 mavlink_message_t msg;
157 mavlink_sys_status_t sysStatus;
158
159 // Sensors present and enabled
160 sysStatus.onboard_control_sensors_present =
161 MAV_SYS_STATUS_SENSOR_3D_GYRO |
162 MAV_SYS_STATUS_SENSOR_3D_ACCEL |
163 MAV_SYS_STATUS_SENSOR_3D_MAG |
164 MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE |
165 MAV_SYS_STATUS_SENSOR_GPS |
166 MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS |
167 MAV_SYS_STATUS_SENSOR_RC_RECEIVER |
168 MAV_SYS_STATUS_SENSOR_BATTERY;
169
170 sysStatus.onboard_control_sensors_enabled = sysStatus.onboard_control_sensors_present;
171
172 // Health status based on actual sensor states
173 sysStatus.onboard_control_sensors_health = 0;
174 if (isIMUHealthy()) sysStatus.onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL;
175 if (isMagHealthy()) sysStatus.onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
176 if (isBaroHealthy()) sysStatus.onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
177 if (isGPSHealthy()) sysStatus.onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
178 if (areMotorsHealthy()) sysStatus.onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
179 if (isRadioHealthy()) sysStatus.onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
180 if (isBatteryHealthy()) sysStatus.onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_BATTERY;
181
182 sysStatus.load = getCPUUsage() * 1000; // Convert to per mille
183 sysStatus.voltage_battery = getBatteryVoltage() * 1000; // Convert to mV
184 sysStatus.current_battery = getBatteryCurrent() * 100; // Convert to cA
185 sysStatus.battery_remaining = getRemainingCapacity() * 100; // Convert to %
186
187 sysStatus.drop_rate_comm = getCommDropRate();
188 sysStatus.errors_comm = getCommErrors();
189 sysStatus.errors_count1 = getSystemError1();
190 sysStatus.errors_count2 = getSystemError2();
191 sysStatus.errors_count3 = getSystemError3();
192 sysStatus.errors_count4 = getSystemError4();
193
194 mavlink_msg_sys_status_encode(systemId, componentId, &msg, &sysStatus);
195 sendMessage(msg);
196 }
197
198 void sendBatteryStatus() {
199 mavlink_message_t msg;
200 mavlink_battery_status_t battery;
201
202 battery.id = 0; // Battery ID
203 battery.battery_function = MAV_BATTERY_FUNCTION_ALL;
204 battery.type = MAV_BATTERY_TYPE_LIPO;
205 battery.temperature = getBatteryTemperature() * 100; // cdegC
206
207 // Individual cell voltages (mV)
208 int cellCount = getBatteryCellCount();
209 for (int i = 0; i < 10; i++) {
210 if (i < cellCount) {
211 battery.voltages[i] = getCellVoltage(i) * 1000;
212 } else {
213 battery.voltages[i] = UINT16_MAX; // Invalid/not present
214 }
215 }
216
217 battery.current_battery = getBatteryCurrent() * 100; // cA
218 battery.current_consumed = getConsumedCapacity(); // mAh
219 battery.energy_consumed = getConsumedEnergy(); // hJ
220 battery.battery_remaining = getRemainingCapacity() * 100; // %
221
222 mavlink_msg_battery_status_encode(systemId, componentId, &msg, &battery);
223 sendMessage(msg);
224 }
225
226 void sendRCChannels() {
227 mavlink_message_t msg;
228 mavlink_rc_channels_t rc;
229
230 rc.time_boot_ms = millis();
231 rc.chancount = 8; // Number of channels
232 rc.chan1_raw = getRCChannel(1);
233 rc.chan2_raw = getRCChannel(2);
234 rc.chan3_raw = getRCChannel(3);
235 rc.chan4_raw = getRCChannel(4);
236 rc.chan5_raw = getRCChannel(5);
237 rc.chan6_raw = getRCChannel(6);
238 rc.chan7_raw = getRCChannel(7);
239 rc.chan8_raw = getRCChannel(8);
240 rc.chan9_raw = 0;
241 rc.chan10_raw = 0;
242 rc.chan11_raw = 0;
243 rc.chan12_raw = 0;
244 rc.chan13_raw = 0;
245 rc.chan14_raw = 0;
246 rc.chan15_raw = 0;
247 rc.chan16_raw = 0;
248 rc.chan17_raw = 0;
249 rc.chan18_raw = 0;
250 rc.rssi = getRadioRSSI();
251
252 mavlink_msg_rc_channels_encode(systemId, componentId, &msg, &rc);
253 sendMessage(msg);
254 }
255
256 void sendVFRHUD() {
257 mavlink_message_t msg;
258 mavlink_vfr_hud_t hud;
259
260 hud.airspeed = getAirSpeed();
261 hud.groundspeed = getGroundSpeed();
262 hud.heading = getCurrentYaw();
263 hud.throttle = getThrottlePercent();
264 hud.alt = getCurrentAltitude();
265 hud.climb = getClimbRate();
266
267 mavlink_msg_vfr_hud_encode(systemId, componentId, &msg, &hud);
268 sendMessage(msg);
269 }
270
271 void sendMissionCurrent() {
272 mavlink_message_t msg;
273 mavlink_mission_current_t missionCurrent;
274
275 missionCurrent.seq = currentWaypointIndex;
276
277 mavlink_msg_mission_current_encode(systemId, componentId, &msg, &missionCurrent);
278 sendMessage(msg);
279 }
280
281 void sendMessage(mavlink_message_t& msg) {
282 uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
283 uint16_t len = mavlink_msg_to_send_buffer(buffer, &msg);
284
285 mavSerial->write(buffer, len);
286 }
287
288 void processIncomingMessages() {
289 while (mavSerial->available()) {
290 uint8_t byte = mavSerial->read();
291 mavlink_message_t msg;
292 mavlink_status_t status;
293
294 if (mavlink_parse_char(MAVLINK_COMM_0, byte, &msg, &status)) {
295 handleIncomingMessage(msg);
296 }
297 }
298 }
299
300 void handleIncomingMessage(mavlink_message_t& msg) {
301 switch (msg.msgid) {
302 case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
303 handleParamRequestList(msg);
304 break;
305
306 case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
307 handleParamRequestRead(msg);
308 break;
309
310 case MAVLINK_MSG_ID_PARAM_SET:
311 handleParamSet(msg);
312 break;
313
314 case MAVLINK_MSG_ID_COMMAND_LONG:
315 handleCommandLong(msg);
316 break;
317
318 case MAVLINK_MSG_ID_SET_MODE:
319 handleSetMode(msg);
320 break;
321
322 case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
323 handleMissionRequestList(msg);
324 break;
325
326 case MAVLINK_MSG_ID_MISSION_COUNT:
327 handleMissionCount(msg);
328 break;
329
330 case MAVLINK_MSG_ID_MISSION_ITEM:
331 handleMissionItem(msg);
332 break;
333
334 case MAVLINK_MSG_ID_MISSION_REQUEST:
335 handleMissionRequest(msg);
336 break;
337
338 case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
339 handleMissionClearAll(msg);
340 break;
341
342 case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
343 handleMissionSetCurrent(msg);
344 break;
345
346 case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
347 handleRCOverride(msg);
348 break;
349
350 default:
351 Serial.printf("Unknown MAVLink message: %d\n", msg.msgid);
352 break;
353 }
354 }
355
356 void handleParamRequestList(mavlink_message_t& msg) {
357 Serial.println("Parameter list requested");
358
359 // Send all flight controller parameters
360 const char* params[][3] = {
361 {"PID_ROLL_P", "1.500", "Real32"},
362 {"PID_ROLL_I", "0.100", "Real32"},
363 {"PID_ROLL_D", "0.050", "Real32"},
364 {"PID_PITCH_P", "1.500", "Real32"},
365 {"PID_PITCH_I", "0.100", "Real32"},
366 {"PID_PITCH_D", "0.050", "Real32"},
367 {"PID_YAW_P", "2.000", "Real32"},
368 {"PID_YAW_I", "0.200", "Real32"},
369 {"PID_YAW_D", "0.000", "Real32"},
370 {"ALT_HOLD_P", "0.500", "Real32"},
371 {"ALT_HOLD_I", "0.100", "Real32"},
372 {"ALT_HOLD_D", "0.200", "Real32"},
373 {"MAX_ANGLE", "30.000", "Real32"},
374 {"MAX_YAW_RATE", "180.000", "Real32"},
375 {"BATT_CELLS", "3", "Int32"},
376 {"BATT_CAPACITY", "5000", "Int32"},
377 {"FAILSAFE_BATT", "10.5", "Real32"},
378 {"FAILSAFE_RC", "2.0", "Real32"},
379 {"GPS_TYPE", "1", "Int32"},
380 {"COMPASS_ENABLE", "1", "Int32"}
381 };
382
383 int paramCount = sizeof(params) / sizeof(params[0]);
384
385 for (int i = 0; i < paramCount; i++) {
386 sendParameter(params[i][0], atof(params[i][1]),
387 strcmp(params[i][2], "Real32") == 0 ? MAV_PARAM_TYPE_REAL32 : MAV_PARAM_TYPE_INT32,
388 i, paramCount);
389 delay(10); // Small delay to prevent flooding
390 }
391 }
392
393 void handleParamSet(mavlink_message_t& msg) {
394 mavlink_param_set_t paramSet;
395 mavlink_msg_param_set_decode(&msg, ¶mSet);
396
397 String paramName = String(paramSet.param_id);
398 float paramValue = paramSet.param_value;
399
400 Serial.printf("Setting parameter %s = %.3f\n", paramName.c_str(), paramValue);
401
402 // Update parameter in flight controller
403 bool success = updateParameter(paramName, paramValue);
404
405 if (success) {
406 // Send parameter value back to confirm
407 sendParameter(paramName, paramValue, paramSet.param_type, 0, 1);
408 } else {
409 Serial.printf("Failed to set parameter %s\n", paramName.c_str());
410 }
411 }
412
413 void handleCommandLong(mavlink_message_t& msg) {
414 mavlink_command_long_t cmd;
415 mavlink_msg_command_long_decode(&msg, &cmd);
416
417 uint8_t result = MAV_RESULT_UNSUPPORTED;
418
419 Serial.printf("Received command: %d\n", cmd.command);
420
421 switch (cmd.command) {
422 case MAV_CMD_COMPONENT_ARM_DISARM:
423 if (cmd.param1 == 1.0) {
424 // Arm command
425 if (isSystemArmable()) {
426 armSystem();
427 result = MAV_RESULT_ACCEPTED;
428 Serial.println("System ARMED via MAVLink");
429 } else {
430 result = MAV_RESULT_FAILED;
431 Serial.println("ARM command rejected - system not armable");
432 }
433 } else {
434 // Disarm command
435 disarmSystem();
436 result = MAV_RESULT_ACCEPTED;
437 Serial.println("System DISARMED via MAVLink");
438 }
439 break;
440
441 case MAV_CMD_NAV_TAKEOFF:
442 if (isArmed()) {
443 float targetAltitude = cmd.param7;
444 startTakeoff(targetAltitude);
445 result = MAV_RESULT_ACCEPTED;
446 Serial.printf("Takeoff to %.1fm initiated\n", targetAltitude);
447 } else {
448 result = MAV_RESULT_FAILED;
449 Serial.println("Takeoff rejected - system not armed");
450 }
451 break;
452
453 case MAV_CMD_NAV_LAND:
454 startLanding();
455 result = MAV_RESULT_ACCEPTED;
456 Serial.println("Landing initiated");
457 break;
458
459 case MAV_CMD_NAV_RETURN_TO_LAUNCH:
460 startReturnToLaunch();
461 result = MAV_RESULT_ACCEPTED;
462 Serial.println("Return to Launch initiated");
463 break;
464
465 case MAV_CMD_MISSION_START:
466 if (currentMission.size() > 0) {
467 missionActive = true;
468 currentWaypointIndex = 0;
469 result = MAV_RESULT_ACCEPTED;
470 Serial.println("Mission started");
471 } else {
472 result = MAV_RESULT_FAILED;
473 Serial.println("No mission loaded");
474 }
475 break;
476
477 case MAV_CMD_DO_SET_MODE:
478 {
479 uint8_t baseMode = (uint8_t)cmd.param1;
480 uint32_t customMode = (uint32_t)cmd.param2;
481 if (setFlightMode(baseMode, customMode)) {
482 result = MAV_RESULT_ACCEPTED;
483 Serial.printf("Flight mode changed to %d\n", customMode);
484 } else {
485 result = MAV_RESULT_FAILED;
486 Serial.println("Mode change rejected");
487 }
488 }
489 break;
490
491 case MAV_CMD_PREFLIGHT_CALIBRATION:
492 if (cmd.param1 == 1.0) {
493 // Gyro calibration
494 startGyroCalibration();
495 result = MAV_RESULT_ACCEPTED;
496 Serial.println("Gyro calibration started");
497 } else if (cmd.param2 == 1.0) {
498 // Magnetometer calibration
499 startMagCalibration();
500 result = MAV_RESULT_ACCEPTED;
501 Serial.println("Magnetometer calibration started");
502 } else if (cmd.param5 == 1.0) {
503 // Accelerometer calibration
504 startAccelCalibration();
505 result = MAV_RESULT_ACCEPTED;
506 Serial.println("Accelerometer calibration started");
507 }
508 break;
509
510 default:
511 result = MAV_RESULT_UNSUPPORTED;
512 Serial.printf("Unsupported command: %d\n", cmd.command);
513 break;
514 }
515
516 // Send command acknowledgment
517 sendCommandAck(cmd.command, result);
518 }
519
520 void handleMissionCount(mavlink_message_t& msg) {
521 mavlink_mission_count_t missionCount;
522 mavlink_msg_mission_count_decode(&msg, &missionCount);
523
524 Serial.printf("Receiving mission with %d waypoints\n", missionCount.count);
525
526 currentMission.clear();
527 currentMission.resize(missionCount.count);
528
529 // Request first mission item
530 if (missionCount.count > 0) {
531 sendMissionRequest(0);
532 }
533 }
534
535 void handleMissionItem(mavlink_message_t& msg) {
536 mavlink_mission_item_t missionItem;
537 mavlink_msg_mission_item_decode(&msg, &missionItem);
538
539 if (missionItem.seq < currentMission.size()) {
540 currentMission[missionItem.seq] = missionItem;
541
542 Serial.printf("Received waypoint %d: %.6f, %.6f, %.1fm\n",
543 missionItem.seq, missionItem.x, missionItem.y, missionItem.z);
544
545 // Request next item
546 if (missionItem.seq + 1 < currentMission.size()) {
547 sendMissionRequest(missionItem.seq + 1);
548 } else {
549 // Mission upload complete
550 sendMissionAck(MAV_MISSION_ACCEPTED);
551 Serial.println("Mission upload completed");
552 }
553 }
554 }
555
556 void sendParameter(const String& name, float value, uint8_t type, uint16_t index, uint16_t count) {
557 mavlink_message_t msg;
558 mavlink_param_value_t param;
559
560 strncpy(param.param_id, name.c_str(), 16);
561 param.param_value = value;
562 param.param_type = type;
563 param.param_count = count;
564 param.param_index = index;
565
566 mavlink_msg_param_value_encode(systemId, componentId, &msg, ¶m);
567 sendMessage(msg);
568 }
569
570 void sendCommandAck(uint16_t command, uint8_t result) {
571 mavlink_message_t msg;
572 mavlink_command_ack_t ack;
573
574 ack.command = command;
575 ack.result = result;
576
577 mavlink_msg_command_ack_encode(systemId, componentId, &msg, &ack);
578 sendMessage(msg);
579 }
580
581 void sendMissionRequest(uint16_t seq) {
582 mavlink_message_t msg;
583 mavlink_mission_request_t request;
584
585 request.target_system = targetSystem;
586 request.target_component = targetComponent;
587 request.seq = seq;
588
589 mavlink_msg_mission_request_encode(systemId, componentId, &msg, &request);
590 sendMessage(msg);
591 }
592
593 void sendMissionAck(uint8_t result) {
594 mavlink_message_t msg;
595 mavlink_mission_ack_t ack;
596
597 ack.target_system = targetSystem;
598 ack.target_component = targetComponent;
599 ack.type = result;
600
601 mavlink_msg_mission_ack_encode(systemId, componentId, &msg, &ack);
602 sendMessage(msg);
603 }
604
605 // Функции-заглушки для интеграции с реальной системой
606 float getCurrentRoll() { return 0.0; }
607 float getCurrentPitch() { return 0.0; }
608 float getCurrentYaw() { return 0.0; }
609 float getCurrentRollRate() { return 0.0; }
610 float getCurrentPitchRate() { return 0.0; }
611 float getCurrentYawRate() { return 0.0; }
612 double getCurrentLatitude() { return 0.0; }
613 double getCurrentLongitude() { return 0.0; }
614 float getCurrentAltitude() { return 0.0; }
615 float getRelativeAltitude() { return 0.0; }
616 float getVelocityX() { return 0.0; }
617 float getVelocityY() { return 0.0; }
618 float getVelocityZ() { return 0.0; }
619 float getGroundSpeed() { return 0.0; }
620 float getAirSpeed() { return 0.0; }
621 float getClimbRate() { return 0.0; }
622 uint8_t getGPSFixType() { return 3; }
623 float getGPSHDOP() { return 1.0; }
624 float getGPSVDOP() { return 1.0; }
625 float getCourseOverGround() { return 0.0; }
626 uint8_t getGPSSatellites() { return 8; }
627 float getBatteryVoltage() { return 12.0; }
628 float getBatteryCurrent() { return 5.0; }
629 uint8_t getBatteryCellCount() { return 3; }
630 float getCellVoltage(int cell) { return 4.0; }
631 float getBatteryTemperature() { return 25.0; }
632 float getRemainingCapacity() { return 0.8; }
633 int getConsumedCapacity() { return 1000; }
634 int getConsumedEnergy() { return 500; }
635 uint16_t getRCChannel(int channel) { return 1500; }
636 uint8_t getRadioRSSI() { return 200; }
637 uint8_t getThrottlePercent() { return 50; }
638 uint8_t getCurrentFlightMode() { return 2; }
639 uint8_t getSystemStatus() { return MAV_STATE_ACTIVE; }
640 float getCPUUsage() { return 0.5; }
641 uint16_t getCommDropRate() { return 0; }
642 uint16_t getCommErrors() { return 0; }
643 uint16_t getSystemError1() { return 0; }
644 uint16_t getSystemError2() { return 0; }
645 uint16_t getSystemError3() { return 0; }
646 uint16_t getSystemError4() { return 0; }
647 bool isArmed() { return false; }
648 bool hasManualInput() { return true; }
649 bool isGuidedMode() { return false; }
650 bool isAutoMode() { return false; }
651 bool isIMUHealthy() { return true; }
652 bool isMagHealthy() { return true; }
653 bool isBaroHealthy() { return true; }
654 bool isGPSHealthy() { return true; }
655 bool areMotorsHealthy() { return true; }
656 bool isRadioHealthy() { return true; }
657 bool isBatteryHealthy() { return true; }
658 bool isSystemArmable() { return true; }
659 bool updateParameter(const String& name, float value) { return true; }
660 bool setFlightMode(uint8_t baseMode, uint32_t customMode) { return true; }
661 void armSystem() {}
662 void disarmSystem() {}
663 void startTakeoff(float altitude) {}
664 void startLanding() {}
665 void startReturnToLaunch() {}
666 void startGyroCalibration() {}
667 void startMagCalibration() {}
668 void startAccelCalibration() {}
669};
670
671// Использование MAVLink системы
672MAVLinkTelemetry mavlink(&Serial2);
673
674void setup() {
675 Serial.begin(115200);
676 mavlink.begin();
677
678 Serial.println("MAVLink Ground Control Station interface ready");
679 Serial.println("Connect with Mission Planner or QGroundControl");
680}
681
682void loop() {
683 mavlink.update();
684 delay(10);
685}
🎯 УРОК 4: ПРАКТИЧЕСКАЯ АТТЕСТАЦИЯ МОДУЛЯ (1.5 часа)
4.1 Комплексный экзамен “Системный инженер дронов”
ТЕОРЕТИЧЕСКАЯ ЧАСТЬ (40% оценки):
1📋 СТРУКТУРА ЭКЗАМЕНА:
2
3БЛОК 1: ТЕОРИЯ УПРАВЛЕНИЯ (25% теории)
4Вопросы:
51. Объясните принцип работы каждой составляющей PID регулятора
62. Как влияет изменение параметров PID на поведение системы?
73. Почему используются каскадные системы управления?
84. Какие ограничения накладываются между уровнями управления?
9
10БЛОК 2: РЕЖИМЫ ПОЛЕТА (35% теории)
11Вопросы:
121. Перечислите условия для перехода между режимами полета
132. Как работает система Failsafe?
143. Объясните принцип навигации по waypoints
154. Какие данные необходимы для автономного полета?
16
17БЛОК 3: ТЕЛЕМЕТРИЯ И ДИАГНОСТИКА (25% теории)
18Вопросы:
191. Какие параметры критичны для безопасности полета?
202. Как определить проблемы в системе по телеметрии?
213. Объясните структуру MAVLink протокола
224. Как настроить Ground Control Station?
23
24БЛОК 4: СИСТЕМНАЯ ИНТЕГРАЦИЯ (15% теории)
25Вопросы:
261. Как все подсистемы взаимодействуют друг с другом?
272. Какие принципы используются для обеспечения отказоустойчивости?
283. Как проводится диагностика всей системы?
29
30Формат: Письменный тест + устная защита решений
31Время: 60 минут письменно + 30 минут устно
32Проходной балл: 80%
ПРАКТИЧЕСКАЯ ЧАСТЬ (60% оценки):
Задание 1: “Настройка системы управления” (30% практики)
1// ЗАДАЧА: Полная настройка автопилота с нуля
2
3/* ТРЕБОВАНИЯ:
41. Настроить PID контроллеры для всех осей
52. Реализовать каскадную систему управления
63. Протестировать все режимы полета
74. Настроить системы безопасности
85. Провести полную диагностику системы
9
10КРИТЕРИИ ОЦЕНКИ:
11- Качество настройки PID (30%)
12- Стабильность всех режимов полета (25%)
13- Правильность работы Failsafe (20%)
14- Качество диагностики (25%)
15
16ВРЕМЯ ВЫПОЛНЕНИЯ: 90 минут
17*/
18
19class FlightControllerSetup {
20public:
21 void performFullSetup() {
22 Serial.println("=== ПОЛНАЯ НАСТРОЙКА АВТОПИЛОТА ===");
23
24 // 1. Калибровка датчиков
25 calibrateAllSensors();
26
27 // 2. Настройка PID контроллеров
28 tunePIDControllers();
29
30 // 3. Настройка режимов полета
31 configureFlightModes();
32
33 // 4. Настройка систем безопасности
34 configureSafetySystems();
35
36 // 5. Проведение тестов
37 runSystemTests();
38
39 // 6. Финальная проверка
40 performFinalValidation();
41 }
42
43private:
44 void calibrateAllSensors() {
45 Serial.println("Калибровка датчиков...");
46
47 // Калибровка IMU
48 Serial.println("1. Калибровка гироскопа...");
49 // Студент должен реализовать калибровку
50
51 Serial.println("2. Калибровка акселерометра...");
52 // 6-точечная калибровка
53
54 Serial.println("3. Калибровка магнитометра...");
55 // Калибровка компаса
56
57 Serial.println("4. Калибровка барометра...");
58 // Установка референсного давления
59 }
60
61 void tunePIDControllers() {
62 Serial.println("Настройка PID контроллеров...");
63
64 // Студент должен настроить все PID
65 tunePIDForAxis("Roll", 1.0, 0.1, 0.05);
66 tunePIDForAxis("Pitch", 1.0, 0.1, 0.05);
67 tunePIDForAxis("Yaw", 2.0, 0.2, 0.0);
68 tunePIDForAxis("Altitude", 0.5, 0.1, 0.2);
69
70 // Проверка качества настройки
71 validatePIDPerformance();
72 }
73
74 void configureFlightModes() {
75 Serial.println("Настройка режимов полета...");
76
77 // Студент должен настроить переходы между режимами
78 setupModeTransitions();
79
80 // Тестирование каждого режима
81 testFlightMode("STABILIZE");
82 testFlightMode("ALT_HOLD");
83 testFlightMode("LOITER");
84 testFlightMode("AUTO");
85 testFlightMode("RTL");
86 }
87
88 void configureSafetySystems() {
89 Serial.println("Настройка систем безопасности...");
90
91 // Failsafe настройки
92 configureRadioFailsafe();
93 configureBatteryFailsafe();
94 configureGPSFailsafe();
95
96 // Геозоны
97 configureGeofence();
98
99 // Аварийные процедуры
100 configureEmergencyProcedures();
101 }
102
103 void runSystemTests() {
104 Serial.println("Проведение системных тестов...");
105
106 // Тест производительности
107 performanceTest();
108
109 // Тест стабильности
110 stabilityTest();
111
112 // Тест безопасности
113 safetyTest();
114
115 // Тест нагрузки
116 loadTest();
117 }
118
119 void performFinalValidation() {
120 Serial.println("Финальная валидация системы...");
121
122 bool allTestsPassed = true;
123
124 // Проверка критических систем
125 if (!validateCriticalSystems()) {
126 allTestsPassed = false;
127 Serial.println("❌ Критические системы не прошли валидацию");
128 }
129
130 // Проверка производительности
131 if (!validatePerformance()) {
132 allTestsPassed = false;
133 Serial.println("❌ Производительность не соответствует требованиям");
134 }
135
136 // Проверка безопасности
137 if (!validateSafety()) {
138 allTestsPassed = false;
139 Serial.println("❌ Системы безопасности работают некорректно");
140 }
141
142 if (allTestsPassed) {
143 Serial.println("✅ ВСЕ ТЕСТЫ ПРОЙДЕНЫ! Система готова к полету.");
144 generateSetupReport();
145 } else {
146 Serial.println("❌ ЕСТЬ ПРОБЛЕМЫ! Требуется дополнительная настройка.");
147 }
148 }
149
150 // Вспомогательные функции (студент должен реализовать)
151 void tunePIDForAxis(String axis, float kp, float ki, float kd) {
152 Serial.printf("Настройка PID для %s: P=%.2f I=%.3f D=%.3f\n", axis.c_str(), kp, ki, kd);
153 // Реализация настройки
154 }
155
156 bool validatePIDPerformance() {
157 // Проверка качества PID настройки
158 return true;
159 }
160
161 void setupModeTransitions() {
162 // Настройка логики переходов между режимами
163 }
164
165 void testFlightMode(String mode) {
166 Serial.printf("Тестирование режима %s...\n", mode.c_str());
167 // Тестирование режима
168 }
169
170 void configureRadioFailsafe() {
171 Serial.println("Настройка Radio Failsafe: потеря сигнала > 2 сек → RTL");
172 }
173
174 void configureBatteryFailsafe() {
175 Serial.println("Настройка Battery Failsafe: напряжение < 10.5V → RTL");
176 }
177
178 void configureGPSFailsafe() {
179 Serial.println("Настройка GPS Failsafe: потеря GPS → ALT_HOLD");
180 }
181
182 void configureGeofence() {
183 Serial.println("Настройка Geofence: радиус 500м, высота 150м");
184 }
185
186 void configureEmergencyProcedures() {
187 Serial.println("Настройка аварийных процедур");
188 }
189
190 bool performanceTest() {
191 Serial.println("Тест производительности: проверка частоты циклов");
192 return true;
193 }
194
195 bool stabilityTest() {
196 Serial.println("Тест стабильности: проверка реакции на возмущения");
197 return true;
198 }
199
200 bool safetyTest() {
201 Serial.println("Тест безопасности: проверка Failsafe систем");
202 return true;
203 }
204
205 bool loadTest() {
206 Serial.println("Тест нагрузки: работа при максимальной нагрузке");
207 return true;
208 }
209
210 bool validateCriticalSystems() {
211 // Проверка критически важных систем
212 return true;
213 }
214
215 bool validatePerformance() {
216 // Проверка производительности
217 return true;
218 }
219
220 bool validateSafety() {
221 // Проверка безопасности
222 return true;
223 }
224
225 void generateSetupReport() {
226 Serial.println("\n=== ОТЧЕТ О НАСТРОЙКЕ СИСТЕМЫ ===");
227 Serial.println("Дата: " + String(__DATE__) + " " + String(__TIME__));
228 Serial.println("Настройщик: [ИМЯ СТУДЕНТА]");
229 Serial.println("Все системы настроены и протестированы");
230 Serial.println("Система готова к эксплуатации");
231 Serial.println("Следующая проверка: через 50 часов полета");
232 }
233};
234
235// Практическое задание для студента
236FlightControllerSetup setupSystem;
237
238void practicalExam() {
239 setupSystem.performFullSetup();
240}
Задание 2: “Диагностика проблем” (30% практики)
1// ЗАДАЧА: Найти и устранить проблемы в системе
2
3/* СЦЕНАРИИ НЕИСПРАВНОСТЕЙ:
41. Дрон дрейфует влево в режиме LOITER
52. Плохое качество стабилизации (колебания)
63. Неточная работа автономных режимов
74. Проблемы с телеметрией
85. Ложные срабатывания Failsafe
9
10ТРЕБУЕТСЯ:
11- Проанализировать логи полета
12- Определить причину каждой проблемы
13- Предложить решение
14- Реализовать исправления
15- Проверить результат
16
17КРИТЕРИИ ОЦЕНКИ:
18- Точность диагностики (40%)
19- Правильность решения (35%)
20- Качество исправления (25%)
21*/
22
23class SystemDiagnostics {
24public:
25 void diagnoseAndFix() {
26 Serial.println("=== ДИАГНОСТИКА СИСТЕМЫ ===");
27
28 // Сбор данных для анализа
29 collectDiagnosticData();
30
31 // Анализ каждой проблемы
32 analyzeStabilizationIssues();
33 analyzeNavigationIssues();
34 analyzeTelemetryIssues();
35 analyzeFailsafeIssues();
36
37 // Генерация отчета
38 generateDiagnosticReport();
39 }
40
41private:
42 void collectDiagnosticData() {
43 Serial.println("Сбор диагностических данных...");
44
45 // Анализ логов
46 analyzeLogs();
47
48 // Проверка датчиков
49 checkSensorHealth();
50
51 // Проверка калибровки
52 checkCalibration();
53
54 // Проверка настроек
55 checkConfiguration();
56 }
57
58 void analyzeStabilizationIssues() {
59 Serial.println("\nАнализ проблем стабилизации:");
60
61 // Симуляция проблем для студента
62 Serial.println("ОБНАРУЖЕНО: Колебания по оси Roll");
63 Serial.println("Возможные причины:");
64 Serial.println("1. Слишком высокий P-коэффициент");
65 Serial.println("2. Недостаточный D-коэффициент");
66 Serial.println("3. Механические проблемы");
67 Serial.println("4. Проблемы с IMU");
68
69 // Студент должен выбрать правильную причину и исправить
70 }
71
72 void analyzeNavigationIssues() {
73 Serial.println("\nАнализ проблем навигации:");
74
75 Serial.println("ОБНАРУЖЕНО: Дрейф в режиме LOITER");
76 Serial.println("Возможные причины:");
77 Serial.println("1. Плохая точность GPS");
78 Serial.println("2. Неправильная калибровка компаса");
79 Serial.println("3. Неправильные PID настройки позиции");
80 Serial.println("4. Влияние магнитных помех");
81 }
82
83 void analyzeTelemetryIssues() {
84 Serial.println("\nАнализ проблем телеметрии:");
85
86 Serial.println("ОБНАРУЖЕНО: Потеря пакетов телеметрии");
87 Serial.println("Возможные причины:");
88 Serial.println("1. Перегрузка канала связи");
89 Serial.println("2. Проблемы с антенной");
90 Serial.println("3. Слишком высокая частота передачи");
91 Serial.println("4. Программные ошибки");
92 }
93
94 void analyzeFailsafeIssues() {
95 Serial.println("\nАнализ проблем Failsafe:");
96
97 Serial.println("ОБНАРУЖЕНО: Ложные срабатывания Battery Failsafe");
98 Serial.println("Возможные причины:");
99 Serial.println("1. Неправильные пороги напряжения");
100 Serial.println("2. Плохие контакты в цепи питания");
101 Serial.println("3. Шумы в измерении напряжения");
102 Serial.println("4. Проблемы с калибровкой ADC");
103 }
104
105 void analyzeLogs() {
106 Serial.println("Анализ логов полета...");
107 // Анализ файлов логов
108 }
109
110 void checkSensorHealth() {
111 Serial.println("Проверка здоровья датчиков...");
112 // Проверка всех датчиков
113 }
114
115 void checkCalibration() {
116 Serial.println("Проверка калибровки...");
117 // Проверка калибровочных данных
118 }
119
120 void checkConfiguration() {
121 Serial.println("Проверка конфигурации...");
122 // Проверка настроек
123 }
124
125 void generateDiagnosticReport() {
126 Serial.println("\n=== ДИАГНОСТИЧЕСКИЙ ОТЧЕТ ===");
127 Serial.println("Найдено проблем: 4");
128 Serial.println("Критических: 1");
129 Serial.println("Предупреждений: 3");
130 Serial.println("\nРекомендации:");
131 Serial.println("1. Снизить P-коэффициент Roll PID");
132 Serial.println("2. Перекалибровать компас");
133 Serial.println("3. Снизить частоту телеметрии");
134 Serial.println("4. Проверить цепи питания");
135 }
136};
4.2 Уровни сертификации:
🏆 SYSTEMS ARCHITECT (90-100 баллов):
- Сертификат “Архитектор систем управления дронов”
- Право на проектирование сложных автопилотов
- Создание собственных алгоритмов управления
- Техническое руководство проектами
⚡ CONTROL SYSTEMS ENGINEER (80-89 баллов):
- Сертификат “Инженер систем управления”
- Настройка и оптимизация автопилотов
- Интеграция новых подсистем
- Диагностика сложных проблем
🎛️ FLIGHT CONTROLLER SPECIALIST (70-79 баллов):
- Сертификат “Специалист по контроллерам полета”
- Настройка стандартных автопилотов
- Базовая диагностика
- Техническая поддержка
🎓 ЗАКЛЮЧЕНИЕ МОДУЛЯ 6
Философское завершение:
“Поздравляю! Вы достигли вершины технического мастерства в области дронов. Теперь вы не просто собираете и программируете дроны — вы создаете интеллектуальные системы, способные принимать решения, адаптироваться к ситуации и выполнять сложные задачи автономно.”
“Путь от простого пилота до системного архитектора завершен. Вы овладели искусством создания ‘думающих’ дронов — систем, которые понимают окружающий мир, планируют свои действия и обеспечивают безопасность полета.”
Достижения Модуля 6:
1🎯 ОСВОЕННЫЕ НАВЫКИ:
2
3ТЕХНИЧЕСКОЕ МАСТЕРСТВО:
4✅ Проектирование многоуровневых систем управления
5✅ Настройка сложных PID регуляторов
6✅ Создание интеллектуальных режимов полета
7✅ Реализация систем навигации и планирования миссий
8
9СИСТЕМНАЯ ИНТЕГРАЦИЯ:
10✅ Объединение всех подсистем в единое целое
11✅ Создание отказоустойчивых архитектур
12✅ Реализация систем мониторинга и диагностики
13✅ Интеграция с наземными станциями управления
14
15ПРАКТИЧЕСКИЕ КОМПЕТЕНЦИИ:
16✅ Диагностика сложных системных проблем
17✅ Оптимизация производительности
18✅ Обеспечение безопасности полета
19✅ Создание пользовательских интерфейсов
Переход к следующему блоку:
БЛОК C: ПРОФЕССИОНАЛЬНОЕ ПРИМЕНЕНИЕ ждет впереди!
- Модуль 7: Коммерческие применения дронов
- Модуль 8: Специализированные системы
- Модуль 9: Проектный модуль
“Техническая база заложена. Теперь время применить ваши знания в реальном мире — от коммерческих проектов до создания собственных инновационных решений!”
🚀 Результат Модуля 6: Студент овладел проектированием и реализацией интеллектуальных систем управления дронами, может создавать сложные автопилоты, интегрировать множественные подсистемы и обеспечивать высокий уровень автономности и безопасности полета.
Урок 6: Специальные техники и “хаки”
1# Продвинутые техники и трюки пилотирования
2
3## 🎯 Точные маневры
4
5### 1. Precision Hovering Techniques
6- **Micro-corrections:** минимальные движения стиков
7- **Pendulum dampening:** гашение колебаний
8- **Wind compensation:** компенсация ветра вручную
9- **Optical flow landing:** посадка по визуальным меткам
10
11### 2. Emergency Procedures
12- **Failsafe recovery:** восстановление после потери сигнала
13- **Motor failure simulation:** полет на трех моторах
14- **Inverted recovery:** восстановление из перевернутого состояния
15- **Autorotation landing:** посадка с выключенными моторами
16
17## 🚀 Трюки и акробатика
18
19### 3. Advanced Aerobatics
20- **Tornado roll:** штопор с набором высоты
21- **Cuban eight:** восьмерка в вертикальной плоскости
22- **Hammer head:** вертикальный подъем с разворотом
23- **Knife edge circle:** круг на ребре винта
24
25### 4. Freestyle элементы
26- **Juicy flick:** резкий флип с остановкой
27- **Matty flip:** сальто назад через препятствие
28- **Wall ride:** полет вдоль вертикальной поверхности
29- **Tree tap:** касание объекта винтом без повреждений