inav-toolkit 2.3.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -0,0 +1,2832 @@
1
+ #!/usr/bin/env python3
2
+ """
3
+ INAV Parameter Analyzer - Analyze `diff all` output for configuration issues.
4
+
5
+ Parses INAV CLI `diff all` output and checks for:
6
+ - Safety issues (beepers, failsafe, voltage limits)
7
+ - Performance issues (filters, PID settings, motor protocol)
8
+ - Missing recommended settings for the hardware profile
9
+ - Cross-reference with blackbox analysis results (optional)
10
+ - Navigation configuration review
11
+
12
+ Usage:
13
+ inav-params diff_all.txt [--frame 10] [--blackbox state.json]
14
+ """
15
+
16
+ import argparse
17
+ import json
18
+ import os
19
+ import re
20
+ import sys
21
+ import textwrap
22
+ from datetime import datetime
23
+
24
+ VERSION = "2.3.0"
25
+
26
+
27
+ def _enable_ansi_colors():
28
+ """Enable ANSI color support. Returns True if colors are available."""
29
+ if os.environ.get("NO_COLOR") is not None:
30
+ return False
31
+ if not hasattr(sys.stdout, "isatty") or not sys.stdout.isatty():
32
+ return False
33
+ if sys.platform == "win32":
34
+ try:
35
+ import ctypes
36
+ kernel32 = ctypes.windll.kernel32
37
+ handle = kernel32.GetStdHandle(-11)
38
+ mode = ctypes.c_ulong()
39
+ kernel32.GetConsoleMode(handle, ctypes.byref(mode))
40
+ kernel32.SetConsoleMode(handle, mode.value | 0x0004)
41
+ return True
42
+ except Exception:
43
+ return False
44
+ return True
45
+
46
+ _ANSI_ENABLED = _enable_ansi_colors()
47
+
48
+ def _colors():
49
+ """Return (R, B, C, G, Y, RED, DIM) color codes."""
50
+ if _ANSI_ENABLED:
51
+ return ("\033[0m", "\033[1m", "\033[96m", "\033[92m",
52
+ "\033[93m", "\033[91m", "\033[2m")
53
+ return ("", "", "", "", "", "", "")
54
+
55
+ def _disable_colors():
56
+ global _ANSI_ENABLED
57
+ _ANSI_ENABLED = False
58
+
59
+ # ─── Severity Levels ─────────────────────────────────────────────────────────
60
+
61
+ CRITICAL = "CRITICAL" # Safety risk, could crash or lose the quad
62
+ WARNING = "WARNING" # Likely performance issue or suboptimal config
63
+ INFO = "INFO" # Suggestion, nice-to-have
64
+ OK = "OK" # Checked and looks good
65
+
66
+ SEVERITY_ORDER = {CRITICAL: 0, WARNING: 1, INFO: 2, OK: 3}
67
+
68
+ # ─── INAV Aux Mode IDs (from src/main/fc/rc_modes.h) ────────────────────────
69
+
70
+ INAV_MODES = {
71
+ 0: "ARM", 1: "ANGLE", 2: "HORIZON", 3: "NAV ALTHOLD",
72
+ 5: "HEADING HOLD", 10: "NAV POSHOLD", 11: "NAV RTH",
73
+ 12: "MANUAL", 13: "NAV WP", 28: "NAV LAUNCH", 45: "TURTLE",
74
+ 47: "OSD ALT", 48: "NAV COURSE HOLD", 53: "MULTI FUNCTION",
75
+ 62: "MIXER PROFILE 2", 63: "MIXER TRANSITION",
76
+ }
77
+
78
+ # Modes that require GPS
79
+ GPS_MODES = {10, 11, 13, 48} # POSHOLD, RTH, WP, COURSE HOLD
80
+ # Modes that require baro or althold
81
+ ALT_MODES = {3} # NAV ALTHOLD
82
+ # Modes that benefit from compass
83
+ COMPASS_MODES = {10, 11, 13, 5} # POSHOLD, RTH, WP, HEADING HOLD
84
+
85
+ # Multirotor platform types
86
+ MC_PLATFORMS = {"MULTIROTOR", "TRICOPTER"}
87
+
88
+ # ─── Known Defaults & Limits ─────────────────────────────────────────────────
89
+
90
+ # INAV 9.x defaults for multirotor (applied_defaults=5 is "multirotor with GPS")
91
+ INAV9_MC_DEFAULTS = {
92
+ "looptime": 500, # 2kHz
93
+ "gyro_main_lpf_hz": 110,
94
+ "gyro_main_lpf_type": "PT1",
95
+ "dterm_lpf_hz": 110,
96
+ "dterm_lpf_type": "PT1",
97
+ "dynamic_gyro_notch_q": 250,
98
+ "dynamic_gyro_notch_min_hz": 80,
99
+ "mc_p_pitch": 44, "mc_i_pitch": 75, "mc_d_pitch": 25,
100
+ "mc_p_roll": 40, "mc_i_roll": 75, "mc_d_roll": 25,
101
+ "mc_p_yaw": 85, "mc_i_yaw": 45, "mc_d_yaw": 0,
102
+ "mc_iterm_relax": "RP",
103
+ "d_boost_min": 1.0, "d_boost_max": 1.0,
104
+ "antigravity_gain": 1.0,
105
+ "motor_pwm_protocol": "DSHOT300",
106
+ "blackbox_rate_denom": 1,
107
+ "failsafe_procedure": "DROP",
108
+ "nav_rth_altitude": 5000,
109
+ "nav_mc_hover_thr": 1500,
110
+ }
111
+
112
+ # Motor pole counts for common motors
113
+ COMMON_MOTOR_POLES = {14: "most standard motors", 12: "some smaller motors"}
114
+
115
+ # ─── Frame-Specific Starting Profiles ────────────────────────────────────────
116
+ # Conservative baselines for large INAV multirotors.
117
+ # These are intentionally soft - designed to be flyable on first hover,
118
+ # then refined with the blackbox analyzer. Too low is sluggish, too high
119
+ # oscillates. We err on the sluggish side.
120
+ #
121
+ # Sources: community experience, INAV defaults scaling, physics of prop inertia.
122
+ # Prop inertia scales roughly with diameter^4, so a 10" prop has ~4x the inertia
123
+ # of a 7". PID gains must scale down accordingly.
124
+
125
+ FRAME_PROFILES = {
126
+ # ── 5-inch baseline ─────────────────────────────────────────────────
127
+ # Sources:
128
+ # - INAV 9 firmware defaults (settings.yaml / pid.c)
129
+ # - INAV configurator 5" preset (GitHub issue #167)
130
+ # - INAV Settings.md: dterm_lpf "100 should work best with 5-inch"
131
+ # - INAV Settings.md: dyn_notch_min "default 150 works best with 5\""
132
+ 5: {
133
+ "name": "5-inch racer / freestyle",
134
+ "description": "Standard FPV size. 2306-2207 motors, 4S-6S.",
135
+ "typical_auw": "350-700g",
136
+ "typical_motors": "2207, 2306, 2405",
137
+ "pids": {
138
+ "mc_p_pitch": 44, "mc_i_pitch": 75, "mc_d_pitch": 28,
139
+ "mc_cd_pitch": 60,
140
+ "mc_p_roll": 40, "mc_i_roll": 60, "mc_d_roll": 26,
141
+ "mc_cd_roll": 60,
142
+ "mc_p_yaw": 45, "mc_i_yaw": 80, "mc_d_yaw": 0,
143
+ "mc_cd_yaw": 60,
144
+ },
145
+ "filters": {
146
+ "gyro_main_lpf_hz": 110,
147
+ "dterm_lpf_hz": 110,
148
+ "dterm_lpf_type": "PT3",
149
+ "dynamic_gyro_notch_min_hz": 150,
150
+ "dynamic_gyro_notch_q": 250,
151
+ "dynamic_gyro_notch_mode": "3D",
152
+ },
153
+ "rates": {
154
+ "roll_rate": 70,
155
+ "pitch_rate": 70,
156
+ "yaw_rate": 60,
157
+ },
158
+ "other": {
159
+ "mc_iterm_relax": "RPY",
160
+ "mc_iterm_relax_cutoff": 15,
161
+ "d_boost_min": 0.8,
162
+ "d_boost_max": 1.2,
163
+ "antigravity_gain": 2.0,
164
+ "antigravity_accelerator": 5.0,
165
+ "tpa_rate": 20,
166
+ "tpa_breakpoint": 1350,
167
+ "rate_accel_limit_roll_pitch": 0,
168
+ "rate_accel_limit_yaw": 0,
169
+ },
170
+ "notes": [
171
+ "Close to INAV 9 defaults. Fine starting point for 5-inch builds.",
172
+ "Tune from here using blackbox - most 5-inch quads converge quickly.",
173
+ ],
174
+ },
175
+ # ── 7-inch ──────────────────────────────────────────────────────────
176
+ # Sources:
177
+ # - INAV Settings.md: dterm_lpf "80 seems like a gold spot for 7-inch"
178
+ # - INAV Settings.md: dyn_notch_min "values around 100 work fine on 7\""
179
+ # - Pawel Spychalski's 7" builds (INAV core dev, community baseline)
180
+ # - INAV defaults designed for 5-7", minimal changes needed at 7"
181
+ 7: {
182
+ "name": "7-inch long-range",
183
+ "description": "Common for long-range builds. 2807-3007 motors, 4S-6S.",
184
+ "typical_auw": "800-1400g",
185
+ "typical_motors": "2807, 2907, 3007",
186
+ "pids": {
187
+ "mc_p_pitch": 44, "mc_i_pitch": 75, "mc_d_pitch": 25,
188
+ "mc_cd_pitch": 60,
189
+ "mc_p_roll": 40, "mc_i_roll": 60, "mc_d_roll": 23,
190
+ "mc_cd_roll": 60,
191
+ "mc_p_yaw": 45, "mc_i_yaw": 80, "mc_d_yaw": 0,
192
+ "mc_cd_yaw": 60,
193
+ },
194
+ "filters": {
195
+ "gyro_main_lpf_hz": 90,
196
+ "dterm_lpf_hz": 80,
197
+ "dterm_lpf_type": "PT3",
198
+ "dynamic_gyro_notch_min_hz": 100,
199
+ "dynamic_gyro_notch_q": 250,
200
+ "dynamic_gyro_notch_mode": "3D",
201
+ },
202
+ "rates": {
203
+ "roll_rate": 50,
204
+ "pitch_rate": 50,
205
+ "yaw_rate": 40,
206
+ },
207
+ "other": {
208
+ "mc_iterm_relax": "RPY",
209
+ "mc_iterm_relax_cutoff": 12,
210
+ "d_boost_min": 0.85,
211
+ "d_boost_max": 1.15,
212
+ "antigravity_gain": 2.0,
213
+ "antigravity_accelerator": 5.0,
214
+ "tpa_rate": 20,
215
+ "tpa_breakpoint": 1350,
216
+ "rate_accel_limit_roll_pitch": 0,
217
+ "rate_accel_limit_yaw": 0,
218
+ },
219
+ "notes": [
220
+ "INAV defaults are designed for 5-7\". Only minor changes needed.",
221
+ "Dterm LPF at 80Hz per INAV dev guidance. Dynamic notch min at 100Hz.",
222
+ "If using triblades, reduce P by 10-15% - they grip harder and respond faster.",
223
+ ],
224
+ },
225
+ # ── 10-inch ─────────────────────────────────────────────────────────
226
+ # Sources:
227
+ # - INAV Settings.md: dyn_notch_min "60-70" for 10", dterm_lpf ~60-70Hz
228
+ # - GitHub #9765 (Ivan): starting PIDs 55/75/35 for 7"+, filters -10Hz from 7"
229
+ # - IntoFPV forum: P:D ratio ~1:1, iterm_relax_cutoff <10 (5 recommended)
230
+ # - Community consensus: frame stiffness 7.5mm arm min, RPM filters critical
231
+ # Conservative starting point biased toward safe first flight.
232
+ 10: {
233
+ "name": "10-inch long-range / cruiser",
234
+ "description": "Long-range with large Li-ion packs. 3507-4010 motors, 4S-6S.",
235
+ "typical_auw": "1500-2800g",
236
+ "typical_motors": "3507, 3508, 4008, 4010",
237
+ "pids": {
238
+ "mc_p_pitch": 40, "mc_i_pitch": 60, "mc_d_pitch": 30,
239
+ "mc_cd_pitch": 40,
240
+ "mc_p_roll": 38, "mc_i_roll": 55, "mc_d_roll": 28,
241
+ "mc_cd_roll": 40,
242
+ "mc_p_yaw": 40, "mc_i_yaw": 60, "mc_d_yaw": 0,
243
+ "mc_cd_yaw": 45,
244
+ },
245
+ "filters": {
246
+ "gyro_main_lpf_hz": 70,
247
+ "dterm_lpf_hz": 60,
248
+ "dterm_lpf_type": "PT3",
249
+ "dynamic_gyro_notch_min_hz": 65,
250
+ "dynamic_gyro_notch_q": 250,
251
+ "dynamic_gyro_notch_mode": "3D",
252
+ },
253
+ "rates": {
254
+ "roll_rate": 36,
255
+ "pitch_rate": 36,
256
+ "yaw_rate": 30,
257
+ },
258
+ "other": {
259
+ "mc_iterm_relax": "RPY",
260
+ "mc_iterm_relax_cutoff": 8,
261
+ "d_boost_min": 0.80,
262
+ "d_boost_max": 1.20,
263
+ "antigravity_gain": 2.0,
264
+ "antigravity_accelerator": 5.0,
265
+ "tpa_rate": 20,
266
+ "tpa_breakpoint": 1250,
267
+ "rate_accel_limit_roll_pitch": 1000,
268
+ "rate_accel_limit_yaw": 500,
269
+ },
270
+ "notes": [
271
+ "P:D ratio close to 1:1 on 10\" (unlike 5\" where P >> D).",
272
+ "Lower iterm_relax_cutoff (8) prevents bounce-back on this heavier airframe.",
273
+ "Rate acceleration limits prevent sudden moves that stress the frame.",
274
+ "Frame stiffness is critical - 7.5mm carbon arms minimum.",
275
+ "If heavy (>2.5kg), reduce P by 10% and raise I by 10% for better position hold.",
276
+ "EZ Tune is an alternative: set ez_filter_hz ~70, lower response/stability.",
277
+ ],
278
+ },
279
+ # ── 12-inch ─────────────────────────────────────────────────────────
280
+ # Sources:
281
+ # - Extrapolated from 10" community data + physics-based scaling
282
+ # - GitHub #9765: 16" Tarot flew P=44/I=45/D=36, gyro=50, dterm=40
283
+ # - Noise frequencies shift down proportionally with prop diameter
284
+ # - INAV Settings.md: accel limits ~360-500 dps^2 for heavy multirotors
285
+ 12: {
286
+ "name": "12-inch heavy lifter",
287
+ "description": "Heavy lift, cine, long endurance. 4510-5010 motors, 6S-12S.",
288
+ "typical_auw": "2500-5000g",
289
+ "typical_motors": "4510, 4515, 5010, 5015",
290
+ "pids": {
291
+ "mc_p_pitch": 32, "mc_i_pitch": 50, "mc_d_pitch": 25,
292
+ "mc_cd_pitch": 30,
293
+ "mc_p_roll": 30, "mc_i_roll": 45, "mc_d_roll": 23,
294
+ "mc_cd_roll": 30,
295
+ "mc_p_yaw": 35, "mc_i_yaw": 50, "mc_d_yaw": 0,
296
+ "mc_cd_yaw": 35,
297
+ },
298
+ "filters": {
299
+ "gyro_main_lpf_hz": 55,
300
+ "dterm_lpf_hz": 45,
301
+ "dterm_lpf_type": "PT3",
302
+ "dynamic_gyro_notch_min_hz": 45,
303
+ "dynamic_gyro_notch_q": 250,
304
+ "dynamic_gyro_notch_mode": "3D",
305
+ },
306
+ "rates": {
307
+ "roll_rate": 28,
308
+ "pitch_rate": 28,
309
+ "yaw_rate": 22,
310
+ },
311
+ "other": {
312
+ "mc_iterm_relax": "RPY",
313
+ "mc_iterm_relax_cutoff": 5,
314
+ "d_boost_min": 0.75,
315
+ "d_boost_max": 1.25,
316
+ "antigravity_gain": 2.5,
317
+ "antigravity_accelerator": 5.0,
318
+ "tpa_rate": 15,
319
+ "tpa_breakpoint": 1300,
320
+ "rate_accel_limit_roll_pitch": 500,
321
+ "rate_accel_limit_yaw": 250,
322
+ },
323
+ "notes": [
324
+ "Large prop inertia makes overcorrection the main risk, not sluggishness.",
325
+ "Aerodynamic damping from large props reduces the need for D-term.",
326
+ "Higher antigravity helps maintain altitude during throttle changes with heavy payloads.",
327
+ "Acceleration limits are essential - sudden rate demands can flex the frame.",
328
+ "FC vibration isolation is critical. Use soft-mount or TPU dampers.",
329
+ "EZ Tune alternative: set ez_filter_hz ~55, lower response/stability.",
330
+ ],
331
+ },
332
+ # ── 15-inch ─────────────────────────────────────────────────────────
333
+ # Sources:
334
+ # - GitHub #9765 (Ivan): gyro filters ~50Hz on 16", PIDs 44/45/36 (tuned)
335
+ # - INAV Settings.md: accel limits 360/180 dps^2 for big heavy multirotors
336
+ # - Physics: prop inertia ~10x that of 7", motor noise 60-120Hz range
337
+ # - Conservative starting point - 16" Tarot flew P=44 but was well-tuned
338
+ 15: {
339
+ "name": "15-inch heavy lift / cine lifter",
340
+ "description": "Maximum endurance and payload. 5515-7015 motors, 6S-12S.",
341
+ "typical_auw": "4000-10000g",
342
+ "typical_motors": "5515, 6010, 6015, 7015",
343
+ "pids": {
344
+ "mc_p_pitch": 28, "mc_i_pitch": 45, "mc_d_pitch": 22,
345
+ "mc_cd_pitch": 20,
346
+ "mc_p_roll": 26, "mc_i_roll": 40, "mc_d_roll": 20,
347
+ "mc_cd_roll": 20,
348
+ "mc_p_yaw": 30, "mc_i_yaw": 40, "mc_d_yaw": 0,
349
+ "mc_cd_yaw": 25,
350
+ },
351
+ "filters": {
352
+ "gyro_main_lpf_hz": 45,
353
+ "dterm_lpf_hz": 35,
354
+ "dterm_lpf_type": "PT3",
355
+ "dynamic_gyro_notch_min_hz": 30,
356
+ "dynamic_gyro_notch_q": 250,
357
+ "dynamic_gyro_notch_mode": "3D",
358
+ },
359
+ "rates": {
360
+ "roll_rate": 20,
361
+ "pitch_rate": 20,
362
+ "yaw_rate": 18,
363
+ },
364
+ "other": {
365
+ "mc_iterm_relax": "RPY",
366
+ "mc_iterm_relax_cutoff": 5,
367
+ "d_boost_min": 0.70,
368
+ "d_boost_max": 1.30,
369
+ "antigravity_gain": 3.0,
370
+ "antigravity_accelerator": 5.0,
371
+ "tpa_rate": 10,
372
+ "tpa_breakpoint": 1300,
373
+ "rate_accel_limit_roll_pitch": 360,
374
+ "rate_accel_limit_yaw": 180,
375
+ },
376
+ "notes": [
377
+ "Prop inertia is enormous. Overcorrection is the main risk, not sluggishness.",
378
+ "Test in calm conditions first. Wind amplifies any PID issues on large frames.",
379
+ "Accel limits (360/180 dps^2) per INAV dev guidance for big heavy multirotors.",
380
+ "FC vibration isolation is critical. Use soft-mount or TPU dampers.",
381
+ "If oscillation persists, lower P before lowering D - unlike smaller quads.",
382
+ "Community data: tuned 16\" flew P=44 - room to increase once vibrations are controlled.",
383
+ "EZ Tune alternative: set ez_filter_hz ~45, lower response/stability significantly.",
384
+ ],
385
+ },
386
+ }
387
+
388
+ # Voltage-specific adjustments (applied on top of frame profile)
389
+ VOLTAGE_ADJUSTMENTS = {
390
+ "4S": {
391
+ "description": "4S (14.8-16.8V)",
392
+ "pid_scale": 1.0, # baseline
393
+ "notes": "Standard voltage. No PID adjustment needed.",
394
+ },
395
+ "6S": {
396
+ "description": "6S (22.2-25.2V)",
397
+ "pid_scale": 0.85, # motors are punchier, need less P
398
+ "notes": "Higher voltage means faster motor response. PIDs scaled down ~15%.",
399
+ },
400
+ "8S": {
401
+ "description": "8S (29.6-33.6V)",
402
+ "pid_scale": 0.75,
403
+ "notes": "Very fast motor response. PIDs scaled down ~25%. Use care on first flight.",
404
+ },
405
+ "12S": {
406
+ "description": "12S (44.4-50.4V)",
407
+ "pid_scale": 0.65,
408
+ "notes": "Extreme voltage. Very conservative PIDs recommended. Motors respond almost instantly.",
409
+ },
410
+ }
411
+
412
+
413
+ # ─── Setup Mode: Generate Starting Config ────────────────────────────────────
414
+
415
+ def generate_setup_config(frame_inches, voltage="4S", use_case="longrange"):
416
+ """Generate conservative starting configuration for a given frame size."""
417
+ if frame_inches not in FRAME_PROFILES:
418
+ # Find nearest
419
+ available = sorted(FRAME_PROFILES.keys())
420
+ nearest = min(available, key=lambda x: abs(x - frame_inches))
421
+ print(f" Warning: No profile for {frame_inches}\". Using nearest: {nearest}\"")
422
+ frame_inches = nearest
423
+
424
+ profile = FRAME_PROFILES[frame_inches]
425
+ v_adj = VOLTAGE_ADJUSTMENTS.get(voltage, VOLTAGE_ADJUSTMENTS["4S"])
426
+ scale = v_adj["pid_scale"]
427
+
428
+ # Scale PIDs by voltage
429
+ pids = {}
430
+ for k, v in profile["pids"].items():
431
+ if v == 0:
432
+ # Respect explicit zeros (e.g. yaw D = 0 on multirotors)
433
+ pids[k] = 0
434
+ elif k.startswith("mc_p_") or k.startswith("mc_d_") or k.startswith("mc_cd_"):
435
+ pids[k] = max(5, round(v * scale))
436
+ elif k.startswith("mc_i_"):
437
+ # I-term scales less with voltage - it's about steady-state
438
+ pids[k] = max(20, round(v * (1.0 + (scale - 1.0) * 0.3)))
439
+ else:
440
+ pids[k] = v
441
+
442
+ config = {
443
+ "frame": frame_inches,
444
+ "voltage": voltage,
445
+ "profile": profile,
446
+ "v_adj": v_adj,
447
+ "pids": pids,
448
+ "filters": dict(profile["filters"]),
449
+ "rates": dict(profile.get("rates", {})),
450
+ "other": dict(profile["other"]),
451
+ }
452
+
453
+ return config
454
+
455
+
456
+ def print_setup_report(config):
457
+ """Print the setup configuration as a readable report with CLI commands."""
458
+ R, B, C, G, Y, _RED, DIM = _colors()
459
+
460
+ profile = config["profile"]
461
+ frame = config["frame"]
462
+ voltage = config["voltage"]
463
+
464
+ print(f"\n{B}{C}{'='*70}{R}")
465
+ print(f"{B}{C} INAV Starting Configuration - {frame}-inch ({voltage}){R}")
466
+ print(f"{B}{C}{'='*70}{R}")
467
+ print(f" {DIM}{profile['description']}{R}")
468
+ print(f" {DIM}Typical AUW: {profile['typical_auw']} | Motors: {profile['typical_motors']}{R}")
469
+
470
+ if config["v_adj"]["pid_scale"] != 1.0:
471
+ pct = (1.0 - config["v_adj"]["pid_scale"]) * 100
472
+ print(f" {Y}Voltage adjustment: {voltage} -> PIDs reduced ~{pct:.0f}%{R}")
473
+
474
+ print(f"\n{B}{C}{'-'*70}{R}")
475
+ print(f" {B}IMPORTANT:{R}")
476
+ print(f" {DIM}These are CONSERVATIVE starting values - safe for first hover.{R}")
477
+ print(f" {DIM}The quad may feel sluggish. That's intentional.{R}")
478
+ print(f" {DIM}Fly, log blackbox data, then use the blackbox analyzer to refine.{R}")
479
+ print(f"{B}{C}{'-'*70}{R}")
480
+
481
+ # PID table
482
+ pids = config["pids"]
483
+ has_cd = any(k.startswith("mc_cd_") for k in pids)
484
+ if has_cd:
485
+ print(f"\n {B}STARTING PIDs:{R}")
486
+ print(f" {'P':>5} {'I':>5} {'D':>5} {'CD':>5}")
487
+ for axis in ("roll", "pitch", "yaw"):
488
+ p = pids.get(f"mc_p_{axis}", "-")
489
+ i = pids.get(f"mc_i_{axis}", "-")
490
+ d = pids.get(f"mc_d_{axis}", "-")
491
+ cd = pids.get(f"mc_cd_{axis}", "-")
492
+ print(f" {axis.capitalize():6} {p:>5} {i:>5} {d:>5} {cd:>5}")
493
+ else:
494
+ print(f"\n {B}STARTING PIDs:{R}")
495
+ print(f" {'P':>5} {'I':>5} {'D':>5}")
496
+ for axis in ("roll", "pitch", "yaw"):
497
+ p = pids.get(f"mc_p_{axis}", "-")
498
+ i = pids.get(f"mc_i_{axis}", "-")
499
+ d = pids.get(f"mc_d_{axis}", "-")
500
+ print(f" {axis.capitalize():6} {p:>5} {i:>5} {d:>5}")
501
+
502
+ # Filters
503
+ f = config["filters"]
504
+ print(f"\n {B}FILTERS:{R}")
505
+ print(f" Gyro LPF: {f['gyro_main_lpf_hz']}Hz")
506
+ dterm_lpf = f.get("dterm_lpf_hz")
507
+ dterm_type = f.get("dterm_lpf_type", "")
508
+ if dterm_lpf:
509
+ type_str = f" ({dterm_type})" if dterm_type else ""
510
+ print(f" D-term LPF: {dterm_lpf}Hz{type_str}")
511
+ print(f" Dynamic notch: {f['dynamic_gyro_notch_mode']} (Q={f['dynamic_gyro_notch_q']}, min={f['dynamic_gyro_notch_min_hz']}Hz)")
512
+
513
+ # Rates
514
+ rates = config.get("rates", {})
515
+ if rates:
516
+ rr = rates.get("roll_rate", "?")
517
+ pr = rates.get("pitch_rate", "?")
518
+ yr = rates.get("yaw_rate", "?")
519
+ # INAV rates are in deca-degrees/sec, so rate 36 = 360 deg/s
520
+ print(f"\n {B}RATES:{R}")
521
+ print(f" Roll: {rr} ({rr*10} deg/s) | Pitch: {pr} ({pr*10} deg/s) | Yaw: {yr} ({yr*10} deg/s)")
522
+
523
+ # Other settings
524
+ print(f"\n {B}RECOMMENDED SETTINGS:{R}")
525
+ o = config["other"]
526
+ relax_cutoff = o.get("mc_iterm_relax_cutoff")
527
+ cutoff_str = f" (cutoff: {relax_cutoff})" if relax_cutoff else ""
528
+ print(f" I-term relax: {o['mc_iterm_relax']}{cutoff_str}")
529
+ print(f" D-boost: {o['d_boost_min']:.2f} - {o['d_boost_max']:.2f}")
530
+ print(f" Antigravity: {o['antigravity_gain']:.1f} (accel: {o['antigravity_accelerator']:.1f})")
531
+ print(f" TPA: {o['tpa_rate']}% above {o['tpa_breakpoint']}")
532
+
533
+ accel_rp = o.get("rate_accel_limit_roll_pitch", 0)
534
+ accel_y = o.get("rate_accel_limit_yaw", 0)
535
+ if accel_rp > 0 or accel_y > 0:
536
+ rp_str = f"{accel_rp} dps^2" if accel_rp > 0 else "OFF"
537
+ y_str = f"{accel_y} dps^2" if accel_y > 0 else "OFF"
538
+ print(f" Accel limits: Roll/Pitch: {rp_str} | Yaw: {y_str}")
539
+
540
+ # Notes
541
+ if profile["notes"]:
542
+ print(f"\n {B}NOTES:{R}")
543
+ for note in profile["notes"]:
544
+ print(f" {DIM}* {note}{R}")
545
+
546
+ if config["v_adj"]["notes"]:
547
+ print(f" {DIM}* {config['v_adj']['notes']}{R}")
548
+
549
+ # CLI commands
550
+ print(f"\n{B}{C}{'-'*70}{R}")
551
+ print(f" {B}INAV CLI - paste into Configurator CLI tab:{R}")
552
+ print(f"{B}{C}{'-'*70}{R}")
553
+ print()
554
+
555
+ all_settings = {}
556
+ all_settings.update(config["pids"])
557
+ all_settings.update(config["filters"])
558
+ all_settings.update(config.get("rates", {}))
559
+ all_settings.update(config["other"])
560
+
561
+ for k, v in all_settings.items():
562
+ if isinstance(v, float):
563
+ print(f" {G}set {k} = {v:.3f}{R}")
564
+ else:
565
+ print(f" {G}set {k} = {v}{R}")
566
+ print(f" {G}save{R}")
567
+
568
+ print(f"\n{B}{C}{'='*70}{R}\n")
569
+
570
+
571
+ def print_setup_json(config):
572
+ """Output setup config as JSON."""
573
+ output = {
574
+ "frame_inches": config["frame"],
575
+ "voltage": config["voltage"],
576
+ "profile_name": config["profile"]["name"],
577
+ "description": config["profile"]["description"],
578
+ "pids": config["pids"],
579
+ "filters": config["filters"],
580
+ "rates": config.get("rates", {}),
581
+ "other": config["other"],
582
+ "notes": config["profile"]["notes"],
583
+ "voltage_notes": config["v_adj"]["notes"],
584
+ }
585
+ print(json.dumps(output, indent=2))
586
+
587
+
588
+ # ─── Parser ──────────────────────────────────────────────────────────────────
589
+
590
+ def parse_diff_all(text):
591
+ """Parse INAV `diff all` output into structured data."""
592
+ result = {
593
+ "version": None,
594
+ "board": None,
595
+ "build_date": None,
596
+ "git_hash": None,
597
+ "master": {}, # global settings
598
+ "control_profiles": {}, # {1: {settings}, 2: {settings}, ...}
599
+ "mixer_profiles": {},
600
+ "battery_profiles": {},
601
+ "active_control_profile": 1,
602
+ "active_mixer_profile": 1,
603
+ "active_battery_profile": 1,
604
+ "features": [],
605
+ "features_disabled": [],
606
+ "beepers_disabled": [],
607
+ "beepers_enabled": [],
608
+ "blackbox_enabled": [],
609
+ "blackbox_disabled": [],
610
+ "serial_ports": {},
611
+ "aux_modes": [],
612
+ "motor_mix": [],
613
+ "raw_text": text,
614
+ }
615
+
616
+ current_section = "master"
617
+ current_profile_type = None
618
+ current_profile_num = 1
619
+
620
+ for line in text.splitlines():
621
+ line = line.strip()
622
+ if not line or line.startswith("#"):
623
+ # Check for version comment
624
+ m = re.match(r"#\s*INAV/(\S+)\s+([\d.]+)\s+(.*?)(?:\s*/\s*(.*))?$", line)
625
+ if m:
626
+ result["board"] = m.group(1)
627
+ result["version"] = m.group(2)
628
+ rest = m.group(3)
629
+ # Extract date and git hash
630
+ dm = re.search(r"(\w+ \d+ \d{4})", rest)
631
+ if dm:
632
+ result["build_date"] = dm.group(1)
633
+ gm = re.search(r"\(([0-9a-f]+)\)", rest)
634
+ if gm:
635
+ result["git_hash"] = gm.group(1)
636
+ continue
637
+
638
+ # Feature lines
639
+ if line.startswith("feature "):
640
+ feat = line[8:].strip()
641
+ if feat.startswith("-"):
642
+ result["features_disabled"].append(feat[1:])
643
+ else:
644
+ result["features"].append(feat)
645
+ continue
646
+
647
+ # Beeper lines
648
+ if line.startswith("beeper "):
649
+ beep = line[7:].strip()
650
+ if beep.startswith("-"):
651
+ result["beepers_disabled"].append(beep[1:])
652
+ else:
653
+ result["beepers_enabled"].append(beep)
654
+ continue
655
+
656
+ # Blackbox field selection
657
+ if line.startswith("blackbox "):
658
+ bb = line[9:].strip()
659
+ if bb.startswith("-"):
660
+ result["blackbox_disabled"].append(bb[1:])
661
+ else:
662
+ result["blackbox_enabled"].append(bb)
663
+ continue
664
+
665
+ # Serial port config
666
+ m = re.match(r"serial\s+(\d+)\s+(.*)", line)
667
+ if m:
668
+ result["serial_ports"][int(m.group(1))] = m.group(2).strip()
669
+ continue
670
+
671
+ # Aux mode
672
+ m = re.match(r"aux\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)", line)
673
+ if m:
674
+ result["aux_modes"].append({
675
+ "index": int(m.group(1)),
676
+ "mode_id": int(m.group(2)),
677
+ "channel": int(m.group(3)),
678
+ "range_low": int(m.group(4)),
679
+ "range_high": int(m.group(5)),
680
+ })
681
+ continue
682
+
683
+ # Motor mix
684
+ m = re.match(r"mmix\s+(\d+)\s+([\d.-]+)\s+([\d.-]+)\s+([\d.-]+)\s+([\d.-]+)", line)
685
+ if m:
686
+ result["motor_mix"].append({
687
+ "index": int(m.group(1)),
688
+ "throttle": float(m.group(2)),
689
+ "roll": float(m.group(3)),
690
+ "pitch": float(m.group(4)),
691
+ "yaw": float(m.group(5)),
692
+ })
693
+ continue
694
+
695
+ # Profile switches
696
+ m = re.match(r"(control_profile|mixer_profile|battery_profile)\s+(\d+)", line)
697
+ if m:
698
+ current_profile_type = m.group(1)
699
+ current_profile_num = int(m.group(2))
700
+ section_map = {
701
+ "control_profile": "control_profiles",
702
+ "mixer_profile": "mixer_profiles",
703
+ "battery_profile": "battery_profiles",
704
+ }
705
+ current_section = section_map[current_profile_type]
706
+ if current_profile_num not in result[current_section]:
707
+ result[current_section][current_profile_num] = {}
708
+ continue
709
+
710
+ # Channel map
711
+ m = re.match(r"map\s+(\w+)", line)
712
+ if m:
713
+ result["master"]["channel_map"] = m.group(1)
714
+ continue
715
+
716
+ # Set commands
717
+ m = re.match(r"set\s+(\S+)\s*=\s*(.*)", line)
718
+ if m:
719
+ key = m.group(1).strip()
720
+ val = m.group(2).strip()
721
+ # Try to parse numeric values
722
+ parsed_val = _parse_value(val)
723
+
724
+ if current_section == "master":
725
+ result["master"][key] = parsed_val
726
+ else:
727
+ result[current_section][current_profile_num][key] = parsed_val
728
+ continue
729
+
730
+ # Active profile restore
731
+ if line.startswith("control_profile") and "restore" not in line:
732
+ m = re.match(r"control_profile\s+(\d+)", line)
733
+ if m:
734
+ result["active_control_profile"] = int(m.group(1))
735
+ if line.startswith("mixer_profile") and "restore" not in line:
736
+ m = re.match(r"mixer_profile\s+(\d+)", line)
737
+ if m:
738
+ result["active_mixer_profile"] = int(m.group(1))
739
+ if line.startswith("battery_profile") and "restore" not in line:
740
+ m = re.match(r"battery_profile\s+(\d+)", line)
741
+ if m:
742
+ result["active_battery_profile"] = int(m.group(1))
743
+
744
+ return result
745
+
746
+
747
+ def _parse_value(val):
748
+ """Parse a value string into int, float, or string."""
749
+ if val.upper() in ("ON", "TRUE", "YES"):
750
+ return True
751
+ if val.upper() in ("OFF", "FALSE", "NO"):
752
+ return False
753
+ try:
754
+ return int(val)
755
+ except ValueError:
756
+ pass
757
+ try:
758
+ return float(val)
759
+ except ValueError:
760
+ pass
761
+ return val
762
+
763
+
764
+ def get_active_control(parsed):
765
+ """Get the active control profile settings merged with master."""
766
+ n = parsed["active_control_profile"]
767
+ profile = parsed["control_profiles"].get(n, {})
768
+ return profile
769
+
770
+
771
+ def get_active_battery(parsed):
772
+ """Get the active battery profile settings."""
773
+ n = parsed["active_battery_profile"]
774
+ return parsed["battery_profiles"].get(n, {})
775
+
776
+
777
+ def get_active_mixer(parsed):
778
+ """Get the active mixer profile settings."""
779
+ n = parsed["active_mixer_profile"]
780
+ return parsed["mixer_profiles"].get(n, {})
781
+
782
+
783
+ def get_setting(parsed, key, default=None):
784
+ """Get a setting value, checking active profile first then master."""
785
+ profile = get_active_control(parsed)
786
+ if key in profile:
787
+ return profile[key]
788
+ if key in parsed["master"]:
789
+ return parsed["master"][key]
790
+ return default
791
+
792
+
793
+ # ─── Rule Engine ─────────────────────────────────────────────────────────────
794
+
795
+ class Finding:
796
+ def __init__(self, severity, category, title, detail, setting=None,
797
+ current=None, recommended=None, cli_fix=None):
798
+ self.severity = severity
799
+ self.category = category
800
+ self.title = title
801
+ self.detail = detail
802
+ self.setting = setting
803
+ self.current = current
804
+ self.recommended = recommended
805
+ self.cli_fix = cli_fix
806
+
807
+ def __repr__(self):
808
+ return f"<{self.severity} {self.category}: {self.title}>"
809
+
810
+
811
+ def run_all_checks(parsed, frame_inches=None, blackbox_state=None):
812
+ """Run all configuration checks and return list of Findings."""
813
+ findings = []
814
+
815
+ findings.extend(check_safety(parsed))
816
+ findings.extend(check_motors_protocol(parsed))
817
+ findings.extend(check_filters(parsed, frame_inches))
818
+ findings.extend(check_pid_config(parsed, frame_inches))
819
+ findings.extend(check_navigation(parsed, frame_inches))
820
+ findings.extend(check_gps(parsed))
821
+ findings.extend(check_blackbox(parsed))
822
+ findings.extend(check_battery(parsed))
823
+ findings.extend(check_rx(parsed))
824
+ findings.extend(check_general(parsed))
825
+
826
+ if blackbox_state:
827
+ findings.extend(check_crossref_blackbox(parsed, blackbox_state))
828
+
829
+ # Sort by severity
830
+ findings.sort(key=lambda f: SEVERITY_ORDER.get(f.severity, 99))
831
+ return findings
832
+
833
+
834
+ # ─── Safety Checks ───────────────────────────────────────────────────────────
835
+
836
+ def check_safety(parsed):
837
+ findings = []
838
+
839
+ # Beeper configuration
840
+ critical_beepers = ["BAT_CRIT_LOW", "BAT_LOW", "RX_LOST", "RX_LOST_LANDING", "HW_FAILURE"]
841
+ disabled = parsed["beepers_disabled"]
842
+ missing_critical = [b for b in critical_beepers if b in disabled]
843
+
844
+ if missing_critical:
845
+ findings.append(Finding(
846
+ CRITICAL, "Safety", "Critical beeper warnings disabled",
847
+ f"These beepers are disabled: {', '.join(missing_critical)}. "
848
+ f"You will get no audible warning for low battery, signal loss, or hardware failure. "
849
+ f"This is extremely dangerous - a silent low battery can cause a crash with no warning.",
850
+ setting="beeper",
851
+ current=f"{len(missing_critical)} critical beepers off",
852
+ recommended="Enable at minimum BAT_CRIT_LOW, BAT_LOW, RX_LOST",
853
+ cli_fix="\n".join(f"beeper {b}" for b in missing_critical)))
854
+
855
+ all_disabled = len(disabled) > 20 # practically all beepers off
856
+ if all_disabled:
857
+ findings.append(Finding(
858
+ WARNING, "Safety", "Almost all beepers disabled",
859
+ f"{len(disabled)} beepers are disabled. While some are cosmetic (ARMING, SYSTEM_INIT), "
860
+ f"consider enabling at least the safety-related ones.",
861
+ setting="beeper",
862
+ current=f"{len(disabled)} disabled"))
863
+
864
+ # Failsafe
865
+ fs_proc = get_setting(parsed, "failsafe_procedure", "DROP")
866
+ if fs_proc == "DROP" or fs_proc == 0:
867
+ findings.append(Finding(
868
+ WARNING, "Safety", "Failsafe set to DROP",
869
+ "On signal loss, the quad will disarm and drop from the sky. "
870
+ "For a GPS-equipped quad, RTH failsafe is much safer.",
871
+ setting="failsafe_procedure",
872
+ current="DROP",
873
+ recommended="RTH",
874
+ cli_fix="set failsafe_procedure = RTH"))
875
+ elif fs_proc == "RTH" or fs_proc == 2:
876
+ findings.append(Finding(
877
+ OK, "Safety", "Failsafe set to RTH",
878
+ "Good - the quad will attempt to return home on signal loss.",
879
+ setting="failsafe_procedure",
880
+ current="RTH"))
881
+
882
+ fs_min_dist = get_setting(parsed, "failsafe_min_distance", 0)
883
+ fs_min_proc = get_setting(parsed, "failsafe_min_distance_procedure", "DROP")
884
+ if fs_min_dist > 0 and (fs_min_proc == "LAND" or fs_min_proc == 1):
885
+ findings.append(Finding(
886
+ OK, "Safety", f"Failsafe min distance: {fs_min_dist/100:.0f}m → LAND",
887
+ f"If within {fs_min_dist/100:.0f}m when signal is lost, the quad will land instead of RTH. Good.",
888
+ setting="failsafe_min_distance"))
889
+ elif fs_min_dist == 0 and (fs_proc == "RTH" or fs_proc == 2):
890
+ findings.append(Finding(
891
+ INFO, "Safety", "No failsafe minimum distance set",
892
+ "Consider setting failsafe_min_distance so the quad lands instead of RTH when close to home. "
893
+ "RTH from 5 meters away can be unpredictable.",
894
+ setting="failsafe_min_distance",
895
+ recommended="set failsafe_min_distance = 1000",
896
+ cli_fix="set failsafe_min_distance = 1000\nset failsafe_min_distance_procedure = LAND"))
897
+
898
+ # DSHOT beeper as alternative
899
+ dshot_beeper = get_setting(parsed, "dshot_beeper_enabled", True)
900
+ motor_protocol = get_setting(parsed, "motor_pwm_protocol", "")
901
+ if isinstance(motor_protocol, str) and "DSHOT" in motor_protocol.upper():
902
+ if dshot_beeper is False:
903
+ if all_disabled or missing_critical:
904
+ findings.append(Finding(
905
+ WARNING, "Safety", "DSHOT beeper also disabled",
906
+ "With physical beeper warnings off AND DSHOT beeper disabled, "
907
+ "you have no way to find the quad if it lands in tall grass. "
908
+ "DSHOT beeper uses the motors to beep - it's your last-resort finder.",
909
+ setting="dshot_beeper_enabled",
910
+ current="OFF",
911
+ recommended="ON",
912
+ cli_fix="set dshot_beeper_enabled = ON"))
913
+
914
+ return findings
915
+
916
+
917
+ # ─── Motor & Protocol Checks ────────────────────────────────────────────────
918
+
919
+ def check_motors_protocol(parsed):
920
+ findings = []
921
+ protocol = get_setting(parsed, "motor_pwm_protocol", "")
922
+ protocol_str = str(protocol).upper()
923
+
924
+ is_dshot = "DSHOT" in protocol_str
925
+ if is_dshot:
926
+ findings.append(Finding(
927
+ OK, "Motors", f"Motor protocol: {protocol}",
928
+ "DSHOT digital protocol - good for consistent motor response.",
929
+ setting="motor_pwm_protocol",
930
+ current=str(protocol)))
931
+ else:
932
+ findings.append(Finding(
933
+ WARNING, "Motors", f"Motor protocol: {protocol}",
934
+ "Not using DSHOT. Digital protocols (DSHOT300/600) give more consistent "
935
+ "motor timing and enable features like DSHOT beeper.",
936
+ setting="motor_pwm_protocol",
937
+ current=str(protocol),
938
+ recommended="DSHOT300 or DSHOT600",
939
+ cli_fix="set motor_pwm_protocol = DSHOT300"))
940
+
941
+ # RPM filter - INAV uses ESC telemetry wire (not bidirectional DSHOT)
942
+ rpm_filter = get_setting(parsed, "rpm_gyro_filter_enabled", None)
943
+
944
+ # Check if ESC telemetry is configured on any serial port
945
+ # Serial function 4096 = ESC telemetry (ESC_SENSOR)
946
+ has_esc_telemetry = any(
947
+ "4096" in conf for conf in parsed["serial_ports"].values()
948
+ )
949
+
950
+ if rpm_filter is True and not has_esc_telemetry:
951
+ findings.append(Finding(
952
+ CRITICAL, "Motors", "RPM filter enabled but no ESC telemetry port configured",
953
+ "INAV's RPM filter requires ESC telemetry data via a dedicated wire from the ESC "
954
+ "to a UART RX pin. No serial port is configured for ESC telemetry (function 4096). "
955
+ "Without RPM data, the filter has nothing to work with and may cause instability.",
956
+ setting="rpm_gyro_filter_enabled",
957
+ current="ON",
958
+ recommended="Connect ESC telemetry wire and configure port, or disable RPM filter",
959
+ cli_fix="set rpm_gyro_filter_enabled = OFF"))
960
+ elif rpm_filter is True and has_esc_telemetry:
961
+ findings.append(Finding(
962
+ OK, "Motors", "RPM filter enabled with ESC telemetry",
963
+ "RPM filter is active and ESC telemetry port is configured. "
964
+ "This provides precise motor noise tracking.",
965
+ setting="rpm_gyro_filter_enabled",
966
+ current="ON"))
967
+
968
+ # Motor stop on low
969
+ motorstop = get_setting(parsed, "motorstop_on_low", True)
970
+ mixer = get_active_mixer(parsed)
971
+ motorstop = mixer.get("motorstop_on_low", motorstop)
972
+ if motorstop is False:
973
+ findings.append(Finding(
974
+ OK, "Motors", "Motor stop on low throttle: OFF",
975
+ "Motors keep spinning at idle - good for flight stability.",
976
+ setting="motorstop_on_low"))
977
+
978
+ # Throttle idle
979
+ battery = get_active_battery(parsed)
980
+ idle = battery.get("throttle_idle", get_setting(parsed, "throttle_idle", 5.0))
981
+ if isinstance(idle, (int, float)):
982
+ if idle < 3.0:
983
+ findings.append(Finding(
984
+ WARNING, "Motors", f"Throttle idle very low: {idle}%",
985
+ "Very low idle may cause desync on some ESCs, especially with high cell count. "
986
+ "5-8% is typical for multirotor.",
987
+ setting="throttle_idle",
988
+ current=f"{idle}%",
989
+ recommended="5.0",
990
+ cli_fix="set throttle_idle = 5.000"))
991
+ elif idle > 10.0:
992
+ findings.append(Finding(
993
+ INFO, "Motors", f"Throttle idle high: {idle}%",
994
+ "High idle wastes battery but can help with prop wash handling. "
995
+ "5-8% is typical.",
996
+ setting="throttle_idle",
997
+ current=f"{idle}%"))
998
+
999
+ return findings
1000
+
1001
+
1002
+ # ─── Filter Checks ───────────────────────────────────────────────────────────
1003
+
1004
+ def check_filters(parsed, frame_inches=None):
1005
+ findings = []
1006
+
1007
+ gyro_lpf = get_setting(parsed, "gyro_main_lpf_hz", 110)
1008
+ dyn_notch_q = get_setting(parsed, "dynamic_gyro_notch_q", 250)
1009
+ dyn_notch_mode = get_setting(parsed, "dynamic_gyro_notch_mode", "3D")
1010
+ dyn_notch_min = get_setting(parsed, "dynamic_gyro_notch_min_hz", 80)
1011
+ kalman_q = get_setting(parsed, "setpoint_kalman_q", 100)
1012
+
1013
+ # EZ Tune detection
1014
+ # ez_enabled controls whether EZ Tune actively computes PIDs.
1015
+ # ez_ parameters may exist in diff even when EZ Tune is disabled (residual from past use).
1016
+ profile = get_active_control(parsed)
1017
+ ez_enabled = profile.get("ez_enabled", get_setting(parsed, "ez_enabled", None))
1018
+ ez_filter = profile.get("ez_filter_hz", get_setting(parsed, "ez_filter_hz", None))
1019
+
1020
+ # EZ Tune is active only if ez_enabled is explicitly ON
1021
+ # If ez_enabled is not in the diff, it's at default (OFF)
1022
+ using_ez = ez_enabled is True
1023
+
1024
+ if using_ez:
1025
+ findings.append(Finding(
1026
+ WARNING, "Filters", f"EZ Tune active (ez_filter_hz = {ez_filter})",
1027
+ "EZ Tune computes PID and filter values from its own parameters (ez_response, "
1028
+ "ez_damping, etc). Manual PID changes via CLI or blackbox analyzer recommendations "
1029
+ "will be OVERWRITTEN on reboot. To apply blackbox tuning: either adjust EZ Tune "
1030
+ "parameters (ez_response, ez_damping, ez_stability), or disable EZ Tune "
1031
+ "and set PIDs manually.",
1032
+ setting="ez_enabled",
1033
+ current="ON",
1034
+ recommended="Disable to use manual PIDs",
1035
+ cli_fix="set ez_enabled = OFF"))
1036
+ elif ez_filter is not None and ez_enabled is None:
1037
+ # ez_ params exist but ez_enabled not in diff (default OFF) - just informational
1038
+ findings.append(Finding(
1039
+ INFO, "Filters", "EZ Tune parameters present but EZ Tune appears disabled",
1040
+ f"Residual EZ Tune parameters found (ez_filter_hz={ez_filter}) but ez_enabled "
1041
+ f"is not set (default OFF). These values are not being used. If you want to clean "
1042
+ f"up, you can leave them - they have no effect while EZ Tune is disabled.",
1043
+ setting="ez_filter_hz",
1044
+ current=f"{ez_filter} (inactive)"))
1045
+
1046
+ # Gyro LPF
1047
+ if isinstance(gyro_lpf, (int, float)):
1048
+ if frame_inches and frame_inches >= 8:
1049
+ if gyro_lpf > 80:
1050
+ findings.append(Finding(
1051
+ WARNING, "Filters", f"Gyro LPF at {gyro_lpf}Hz - high for {frame_inches}-inch",
1052
+ f"Large props generate noise at lower frequencies. For {frame_inches}-inch, "
1053
+ f"gyro LPF between 40-80Hz is typical. Higher values let more motor noise "
1054
+ f"reach the PID controller.",
1055
+ setting="gyro_main_lpf_hz",
1056
+ current=str(gyro_lpf),
1057
+ recommended="40-80",
1058
+ cli_fix=f"set gyro_main_lpf_hz = 65"))
1059
+ else:
1060
+ findings.append(Finding(
1061
+ OK, "Filters", f"Gyro LPF at {gyro_lpf}Hz - reasonable for {frame_inches}-inch",
1062
+ "Filter cutoff is in the expected range for this prop size.",
1063
+ setting="gyro_main_lpf_hz",
1064
+ current=str(gyro_lpf)))
1065
+ elif frame_inches and frame_inches <= 5:
1066
+ if gyro_lpf < 80:
1067
+ findings.append(Finding(
1068
+ INFO, "Filters", f"Gyro LPF at {gyro_lpf}Hz - conservative for {frame_inches}-inch",
1069
+ "This is quite low for a small quad. You might be losing responsiveness. "
1070
+ "90-150Hz is typical for 5-inch unless you have vibration issues.",
1071
+ setting="gyro_main_lpf_hz",
1072
+ current=str(gyro_lpf)))
1073
+
1074
+ # Dynamic notch
1075
+ if dyn_notch_mode and str(dyn_notch_mode).upper() not in ("OFF", "0", "FALSE"):
1076
+ findings.append(Finding(
1077
+ OK, "Filters", f"Dynamic notch filter: {dyn_notch_mode} (Q={dyn_notch_q}, min={dyn_notch_min}Hz)",
1078
+ "Dynamic notch tracks motor noise peaks in real-time.",
1079
+ setting="dynamic_gyro_notch_mode",
1080
+ current=f"{dyn_notch_mode}"))
1081
+
1082
+ if isinstance(dyn_notch_min, (int, float)) and frame_inches is not None:
1083
+ # INAV docs: 150 for 5", 100 for 7", 60-70 for 10"
1084
+ expected_max = {5: 180, 7: 120, 10: 80, 12: 60, 15: 45}
1085
+ nearest_size = min(expected_max.keys(), key=lambda x: abs(x - frame_inches))
1086
+ max_thresh = expected_max[nearest_size]
1087
+ if frame_inches >= 8 and dyn_notch_min > max_thresh:
1088
+ rec = max_thresh - 10
1089
+ findings.append(Finding(
1090
+ INFO, "Filters", f"Dynamic notch min_hz = {dyn_notch_min}Hz - may miss low-freq noise",
1091
+ f"Large props can produce harmonics below {dyn_notch_min}Hz. "
1092
+ f"INAV docs recommend lower values for {frame_inches}-inch. "
1093
+ f"Consider lowering to {rec}-{max_thresh}Hz.",
1094
+ setting="dynamic_gyro_notch_min_hz",
1095
+ current=str(dyn_notch_min),
1096
+ recommended=f"{rec}-{max_thresh}",
1097
+ cli_fix=f"set dynamic_gyro_notch_min_hz = {rec}"))
1098
+ else:
1099
+ findings.append(Finding(
1100
+ WARNING, "Filters", "Dynamic notch filter is OFF",
1101
+ "The dynamic notch filter tracks motor noise harmonics in real-time. "
1102
+ "Without it, you rely entirely on the lowpass filter which also adds phase lag. "
1103
+ "Strongly recommended for all INAV multirotor builds.",
1104
+ setting="dynamic_gyro_notch_mode",
1105
+ current="OFF",
1106
+ recommended="3D",
1107
+ cli_fix="set dynamic_gyro_notch_mode = 3D"))
1108
+
1109
+ # D-term LPF
1110
+ dterm_lpf = get_setting(parsed, "dterm_lpf_hz", 110)
1111
+ if isinstance(dterm_lpf, (int, float)) and frame_inches:
1112
+ # Expected ranges per frame size from INAV dev guidance and community data
1113
+ expected = {5: (90, 150), 7: (70, 100), 10: (45, 75), 12: (35, 60), 15: (25, 45)}
1114
+ nearest = min(expected.keys(), key=lambda x: abs(x - frame_inches))
1115
+ low, high = expected[nearest]
1116
+ if dterm_lpf > high + 20:
1117
+ findings.append(Finding(
1118
+ WARNING, "Filters", f"D-term LPF at {dterm_lpf}Hz - high for {frame_inches}-inch",
1119
+ f"Large props generate lower-frequency noise that feeds through D-term. "
1120
+ f"For {frame_inches}-inch, {low}-{high}Hz is typical. "
1121
+ f"High D-term filtering lets motor noise through and causes hot motors.",
1122
+ setting="dterm_lpf_hz",
1123
+ current=str(dterm_lpf),
1124
+ recommended=f"{low}-{high}",
1125
+ cli_fix=f"set dterm_lpf_hz = {(low + high) // 2}"))
1126
+ elif dterm_lpf < low - 15:
1127
+ findings.append(Finding(
1128
+ INFO, "Filters", f"D-term LPF at {dterm_lpf}Hz - conservative for {frame_inches}-inch",
1129
+ f"This adds more phase lag than needed. For {frame_inches}-inch, {low}-{high}Hz "
1130
+ f"is typical. Higher values reduce delay and improve prop wash handling.",
1131
+ setting="dterm_lpf_hz",
1132
+ current=str(dterm_lpf)))
1133
+
1134
+ # Setpoint Kalman
1135
+ if isinstance(kalman_q, (int, float)):
1136
+ if kalman_q > 300:
1137
+ findings.append(Finding(
1138
+ INFO, "Filters", f"Setpoint Kalman Q={kalman_q} - very aggressive",
1139
+ "High Kalman Q means less smoothing on the setpoint. "
1140
+ "This can make the quad feel very snappy but also amplify RC noise.",
1141
+ setting="setpoint_kalman_q",
1142
+ current=str(kalman_q)))
1143
+
1144
+ return findings
1145
+
1146
+
1147
+ # ─── PID Configuration Checks ───────────────────────────────────────────────
1148
+
1149
+ def check_pid_config(parsed, frame_inches=None):
1150
+ findings = []
1151
+ profile = get_active_control(parsed)
1152
+ pnum = parsed["active_control_profile"]
1153
+
1154
+ # Check if EZ Tune is active in this profile
1155
+ ez_active = profile.get("ez_enabled", get_setting(parsed, "ez_enabled", None)) is True
1156
+ ez_note = " EZ Tune does NOT control this setting - it needs to be set manually." if ez_active else ""
1157
+
1158
+ # I-term relax
1159
+ iterm_relax = profile.get("mc_iterm_relax", get_setting(parsed, "mc_iterm_relax", "RP"))
1160
+ other_profiles_have_rpy = False
1161
+ for n, p in parsed["control_profiles"].items():
1162
+ if n != pnum and p.get("mc_iterm_relax") == "RPY":
1163
+ other_profiles_have_rpy = True
1164
+ break
1165
+
1166
+ if str(iterm_relax).upper() == "RP" and other_profiles_have_rpy:
1167
+ findings.append(Finding(
1168
+ WARNING, "PID", f"Active profile {pnum} uses iterm_relax=RP, other profiles use RPY",
1169
+ "I-term relax on yaw (RPY) reduces I-term windup during fast yaw moves, "
1170
+ "preventing yaw bounce-back. Your other profiles have RPY enabled but the active one doesn't. "
1171
+ f"This looks like a configuration oversight.{ez_note}",
1172
+ setting="mc_iterm_relax",
1173
+ current="RP",
1174
+ recommended="RPY",
1175
+ cli_fix="set mc_iterm_relax = RPY"))
1176
+ elif str(iterm_relax).upper() == "RP":
1177
+ findings.append(Finding(
1178
+ INFO, "PID", "I-term relax is RP (roll/pitch only)",
1179
+ "Consider RPY if you experience yaw bounce-back on fast rotations.",
1180
+ setting="mc_iterm_relax",
1181
+ current="RP",
1182
+ recommended="RPY"))
1183
+
1184
+ # I-term relax cutoff - critical for large quads
1185
+ relax_cutoff = profile.get("mc_iterm_relax_cutoff",
1186
+ get_setting(parsed, "mc_iterm_relax_cutoff", 15))
1187
+ if isinstance(relax_cutoff, (int, float)) and frame_inches and frame_inches >= 9:
1188
+ if relax_cutoff > 10:
1189
+ recommended_cutoff = 8 if frame_inches <= 10 else 5
1190
+ findings.append(Finding(
1191
+ WARNING, "PID",
1192
+ f"I-term relax cutoff = {relax_cutoff} - high for {frame_inches}-inch",
1193
+ f"Large quads need lower iterm_relax_cutoff to prevent bounce-back. "
1194
+ f"The default of 15 works for 5\", but {frame_inches}\" builds "
1195
+ f"need {recommended_cutoff} or lower. High cutoff lets I-term build "
1196
+ f"during maneuvers, causing overshoot on heavy airframes.{ez_note}",
1197
+ setting="mc_iterm_relax_cutoff",
1198
+ current=str(relax_cutoff),
1199
+ recommended=str(recommended_cutoff),
1200
+ cli_fix=f"set mc_iterm_relax_cutoff = {recommended_cutoff}"))
1201
+
1202
+ # D-boost
1203
+ d_boost_min = profile.get("d_boost_min", get_setting(parsed, "d_boost_min", 1.0))
1204
+ d_boost_max = profile.get("d_boost_max", get_setting(parsed, "d_boost_max", 1.0))
1205
+ other_profiles_have_dboost = False
1206
+ for n, p in parsed["control_profiles"].items():
1207
+ if n != pnum and (p.get("d_boost_min", 1.0) != 1.0 or p.get("d_boost_max", 1.0) != 1.0):
1208
+ other_profiles_have_dboost = True
1209
+ break
1210
+
1211
+ if d_boost_min == 1.0 and d_boost_max == 1.0 and other_profiles_have_dboost:
1212
+ findings.append(Finding(
1213
+ WARNING, "PID", f"Active profile {pnum} has no D-boost, other profiles do",
1214
+ "D-boost dynamically adjusts D-term based on setpoint changes - it gives more D during "
1215
+ "fast maneuvers (reduces overshoot) and less during straight flight (less noise). "
1216
+ "Your other profiles have it configured but the active one uses defaults. "
1217
+ f"Likely a configuration oversight.{ez_note}",
1218
+ setting="d_boost_min / d_boost_max",
1219
+ current="1.0 / 1.0 (disabled)",
1220
+ recommended="0.8 / 1.2 (like your other profiles)",
1221
+ cli_fix="set d_boost_min = 0.800\nset d_boost_max = 1.200"))
1222
+
1223
+ # Antigravity
1224
+ antigrav = profile.get("antigravity_gain", get_setting(parsed, "antigravity_gain", 1.0))
1225
+ other_have_antigrav = any(
1226
+ p.get("antigravity_gain", 1.0) != 1.0
1227
+ for n, p in parsed["control_profiles"].items() if n != pnum
1228
+ )
1229
+
1230
+ if antigrav == 1.0 and other_have_antigrav:
1231
+ findings.append(Finding(
1232
+ WARNING, "PID", f"Active profile {pnum} has no antigravity, other profiles do",
1233
+ "Antigravity boosts I-term during rapid throttle changes to counteract the "
1234
+ "altitude drop/gain when you punch or chop throttle. Your other profiles have it "
1235
+ f"but the active one doesn't.{ez_note}",
1236
+ setting="antigravity_gain",
1237
+ current="1.0 (disabled)",
1238
+ recommended="2.0 (like your other profiles)",
1239
+ cli_fix="set antigravity_gain = 2.000\nset antigravity_accelerator = 5.000"))
1240
+
1241
+ # TPA
1242
+ tpa_rate = profile.get("tpa_rate", get_setting(parsed, "tpa_rate", 0))
1243
+ tpa_bp = profile.get("tpa_breakpoint", get_setting(parsed, "tpa_breakpoint", 1500))
1244
+ if isinstance(tpa_rate, (int, float)) and tpa_rate > 0:
1245
+ if isinstance(tpa_bp, (int, float)) and tpa_bp < 1300:
1246
+ findings.append(Finding(
1247
+ INFO, "PID", f"TPA breakpoint at {tpa_bp} - quite low",
1248
+ f"TPA starts reducing PID gains at throttle {tpa_bp}. This means gains start "
1249
+ f"dropping early. For aggressive flying, 1350-1500 is more common.",
1250
+ setting="tpa_breakpoint",
1251
+ current=str(tpa_bp),
1252
+ recommended="1350-1500"))
1253
+
1254
+ # Rate acceleration limits - important for large quads
1255
+ if frame_inches and frame_inches >= 9:
1256
+ accel_rp = get_setting(parsed, "rate_accel_limit_roll_pitch", 0)
1257
+ accel_yaw = get_setting(parsed, "rate_accel_limit_yaw", 0)
1258
+ if isinstance(accel_rp, (int, float)) and accel_rp == 0:
1259
+ rec = 500 if frame_inches <= 10 else 360
1260
+ findings.append(Finding(
1261
+ INFO, "PID",
1262
+ f"No roll/pitch acceleration limit set for {frame_inches}-inch quad",
1263
+ f"Large quads benefit from rate acceleration limits to prevent sudden "
1264
+ f"rate demands that stress the frame and overwhelm the motors. "
1265
+ f"INAV devs recommend ~360 dps^2 for big heavy multirotors. "
1266
+ f"A value around {rec} is a good starting point for {frame_inches}\".{ez_note}",
1267
+ setting="rate_accel_limit_roll_pitch",
1268
+ current="0 (unlimited)",
1269
+ recommended=str(rec),
1270
+ cli_fix=f"set rate_accel_limit_roll_pitch = {rec}"))
1271
+
1272
+ # EZ Tune cross-check (only relevant if EZ Tune is actually active)
1273
+ if ez_active:
1274
+ ez_damping = profile.get("ez_damping", get_setting(parsed, "ez_damping", 100))
1275
+ if isinstance(ez_damping, (int, float)) and ez_damping > 120:
1276
+ findings.append(Finding(
1277
+ INFO, "PID", f"EZ Tune damping = {ez_damping} (high)",
1278
+ "High EZ damping means more D-term. This can help with overshoot but "
1279
+ "also amplifies noise. If blackbox shows clean gyro, this is fine.",
1280
+ setting="ez_damping",
1281
+ current=str(ez_damping)))
1282
+
1283
+ return findings
1284
+
1285
+
1286
+ # ─── Navigation Checks ───────────────────────────────────────────────────────
1287
+
1288
+ def check_navigation(parsed, frame_inches=None):
1289
+ findings = []
1290
+
1291
+ rth_alt = get_setting(parsed, "nav_rth_altitude", 5000)
1292
+ hover_thr = get_setting(parsed, "nav_mc_hover_thr", 1500)
1293
+ profile = get_active_control(parsed)
1294
+
1295
+ # Safehome
1296
+ has_safehome = any("safehome" in line.lower() and "set" in line.lower()
1297
+ for line in parsed["raw_text"].splitlines()
1298
+ if not line.strip().startswith("#"))
1299
+ if not has_safehome:
1300
+ findings.append(Finding(
1301
+ INFO, "Navigation", "No safehome configured",
1302
+ "Safehome lets you define alternative landing points for RTH. "
1303
+ "If you fly at a regular location, setting a safehome ensures the quad "
1304
+ "returns to a safe landing spot even if the home point drifted.",
1305
+ setting="safehome",
1306
+ recommended="Configure via Configurator Mission tab"))
1307
+
1308
+ # RTH altitude
1309
+ if isinstance(rth_alt, (int, float)):
1310
+ alt_m = rth_alt / 100
1311
+ if alt_m > 100:
1312
+ findings.append(Finding(
1313
+ WARNING, "Navigation", f"RTH altitude: {alt_m:.0f}m - very high",
1314
+ "High RTH altitude means longer return time and more battery used. "
1315
+ "Also may exceed legal altitude limits in many countries (120m/400ft).",
1316
+ setting="nav_rth_altitude",
1317
+ current=f"{alt_m:.0f}m",
1318
+ recommended="30-60m"))
1319
+ elif alt_m < 15:
1320
+ findings.append(Finding(
1321
+ WARNING, "Navigation", f"RTH altitude: {alt_m:.0f}m - quite low",
1322
+ "Low RTH altitude risks collision with trees, buildings, or terrain. "
1323
+ "30-50m is typical for safe RTH.",
1324
+ setting="nav_rth_altitude",
1325
+ current=f"{alt_m:.0f}m",
1326
+ recommended="30-50m"))
1327
+ else:
1328
+ findings.append(Finding(
1329
+ OK, "Navigation", f"RTH altitude: {alt_m:.0f}m",
1330
+ "Reasonable RTH altitude.",
1331
+ setting="nav_rth_altitude",
1332
+ current=f"{alt_m:.0f}m"))
1333
+
1334
+ # Hover throttle
1335
+ battery = get_active_battery(parsed)
1336
+ hover = battery.get("nav_mc_hover_thr", hover_thr)
1337
+ if isinstance(hover, (int, float)):
1338
+ if hover > 1600:
1339
+ findings.append(Finding(
1340
+ WARNING, "Navigation", f"Hover throttle: {hover} - high",
1341
+ "Hover throttle above 1600 means the quad is heavy relative to motor power. "
1342
+ "Altitude hold and nav may be sluggish because there's limited headroom for correction.",
1343
+ setting="nav_mc_hover_thr",
1344
+ current=str(hover),
1345
+ recommended="1200-1500"))
1346
+ elif hover < 1100:
1347
+ findings.append(Finding(
1348
+ INFO, "Navigation", f"Hover throttle: {hover} - very low",
1349
+ "This suggests very powerful motors relative to weight. "
1350
+ "Altitude hold may be twitchy. Consider lowering PID gains for nav.",
1351
+ setting="nav_mc_hover_thr",
1352
+ current=str(hover)))
1353
+
1354
+ # ── Frame-aware nav PID checks ──
1355
+ # INAV defaults are tuned for 5" quads. Larger frames have more inertia
1356
+ # and need softer nav PIDs and longer deceleration time. The default
1357
+ # values cause oscillation on deceleration, overshoot on RTH arrival,
1358
+ # and bouncy position hold on 10"+ frames.
1359
+ #
1360
+ # Recommended ranges by frame size:
1361
+ # 5": pos_p=50-65 vel_p=35-50 vel_d=80-120 decel=100-150
1362
+ # 7": pos_p=40-55 vel_p=30-40 vel_d=80-120 decel=120-180
1363
+ # 10": pos_p=30-45 vel_p=20-30 vel_d=80-120 decel=180-280
1364
+ # 12": pos_p=25-40 vel_p=15-25 vel_d=80-120 decel=220-350
1365
+ # 15": pos_p=20-35 vel_p=10-20 vel_d=80-120 decel=280-400
1366
+
1367
+ nav_rec = None
1368
+ if frame_inches and frame_inches >= 7:
1369
+ if frame_inches >= 15:
1370
+ nav_rec = {"pos_p_max": 35, "vel_p_max": 20, "decel_min": 280,
1371
+ "pos_p_rec": 25, "vel_p_rec": 15, "vel_i_rec": 8, "decel_rec": 350}
1372
+ elif frame_inches >= 12:
1373
+ nav_rec = {"pos_p_max": 40, "vel_p_max": 25, "decel_min": 220,
1374
+ "pos_p_rec": 30, "vel_p_rec": 20, "vel_i_rec": 10, "decel_rec": 280}
1375
+ elif frame_inches >= 10:
1376
+ nav_rec = {"pos_p_max": 45, "vel_p_max": 30, "decel_min": 180,
1377
+ "pos_p_rec": 40, "vel_p_rec": 25, "vel_i_rec": 10, "decel_rec": 250}
1378
+ elif frame_inches >= 7:
1379
+ nav_rec = {"pos_p_max": 55, "vel_p_max": 40, "decel_min": 120,
1380
+ "pos_p_rec": 45, "vel_p_rec": 35, "vel_i_rec": 15, "decel_rec": 150}
1381
+
1382
+ # INAV defaults for nav PIDs (these won't appear in diff all if unchanged)
1383
+ INAV_NAV_DEFAULTS = {
1384
+ "nav_mc_pos_xy_p": 65,
1385
+ "nav_mc_vel_xy_p": 40,
1386
+ "nav_mc_vel_xy_i": 15,
1387
+ "nav_mc_pos_deceleration_time": 120,
1388
+ "nav_mc_heading_p": 60,
1389
+ }
1390
+
1391
+ pos_p = profile.get("nav_mc_pos_xy_p",
1392
+ get_setting(parsed, "nav_mc_pos_xy_p", INAV_NAV_DEFAULTS["nav_mc_pos_xy_p"]))
1393
+ vel_p = profile.get("nav_mc_vel_xy_p",
1394
+ get_setting(parsed, "nav_mc_vel_xy_p", INAV_NAV_DEFAULTS["nav_mc_vel_xy_p"]))
1395
+ vel_i = profile.get("nav_mc_vel_xy_i",
1396
+ get_setting(parsed, "nav_mc_vel_xy_i", INAV_NAV_DEFAULTS["nav_mc_vel_xy_i"]))
1397
+ decel = get_setting(parsed, "nav_mc_pos_deceleration_time",
1398
+ INAV_NAV_DEFAULTS["nav_mc_pos_deceleration_time"])
1399
+ heading_p = profile.get("nav_mc_heading_p",
1400
+ get_setting(parsed, "nav_mc_heading_p", INAV_NAV_DEFAULTS["nav_mc_heading_p"]))
1401
+
1402
+ if nav_rec:
1403
+ is_default_pos = (pos_p == INAV_NAV_DEFAULTS["nav_mc_pos_xy_p"])
1404
+ is_default_vel = (vel_p == INAV_NAV_DEFAULTS["nav_mc_vel_xy_p"])
1405
+ is_default_decel = (decel == INAV_NAV_DEFAULTS["nav_mc_pos_deceleration_time"])
1406
+ default_note = " (INAV default - tuned for 5-inch)"
1407
+
1408
+ # Frame-aware checks
1409
+ if pos_p is not None and isinstance(pos_p, (int, float)):
1410
+ if pos_p > nav_rec["pos_p_max"]:
1411
+ val_note = default_note if is_default_pos else ""
1412
+ findings.append(Finding(
1413
+ WARNING, "Navigation",
1414
+ f"Position P = {pos_p}{val_note} - too aggressive for {frame_inches}-inch",
1415
+ f"On {frame_inches}-inch with more "
1416
+ f"inertia, high position P causes overshoot and oscillation on RTH arrival "
1417
+ f"and position hold. The quad overshoots the target position, corrects back, "
1418
+ f"overshoots again.",
1419
+ setting="nav_mc_pos_xy_p",
1420
+ current=str(pos_p),
1421
+ recommended=str(nav_rec["pos_p_rec"]),
1422
+ cli_fix=f"set nav_mc_pos_xy_p = {nav_rec['pos_p_rec']}"))
1423
+
1424
+ if vel_p is not None and isinstance(vel_p, (int, float)):
1425
+ if vel_p > nav_rec["vel_p_max"]:
1426
+ val_note = default_note if is_default_vel else ""
1427
+ findings.append(Finding(
1428
+ WARNING, "Navigation",
1429
+ f"Velocity XY P = {vel_p}{val_note} - too aggressive for {frame_inches}-inch",
1430
+ f"Controls how hard the quad brakes when decelerating. "
1431
+ f"On {frame_inches}-inch, the quad can't stop as fast due to momentum, "
1432
+ f"so high velocity P causes oscillation in the direction of travel when "
1433
+ f"stopping or changing direction.",
1434
+ setting="nav_mc_vel_xy_p",
1435
+ current=str(vel_p),
1436
+ recommended=str(nav_rec["vel_p_rec"]),
1437
+ cli_fix=f"set nav_mc_vel_xy_p = {nav_rec['vel_p_rec']}"))
1438
+
1439
+ if decel is not None and isinstance(decel, (int, float)):
1440
+ if decel < nav_rec["decel_min"]:
1441
+ val_note = default_note if is_default_decel else ""
1442
+ findings.append(Finding(
1443
+ WARNING, "Navigation",
1444
+ f"Deceleration time = {decel} ({decel/100:.1f}s){val_note} - too short for {frame_inches}-inch",
1445
+ f"This controls how quickly the quad tries to stop from cruise speed. "
1446
+ f"A {frame_inches}-inch has much more "
1447
+ f"momentum and needs more distance/time to decelerate smoothly. "
1448
+ f"Too short causes overshoot and oscillation on RTH and position hold transitions.",
1449
+ setting="nav_mc_pos_deceleration_time",
1450
+ current=f"{decel} ({decel/100:.1f}s)",
1451
+ recommended=f"{nav_rec['decel_rec']} ({nav_rec['decel_rec']/100:.1f}s)",
1452
+ cli_fix=f"set nav_mc_pos_deceleration_time = {nav_rec['decel_rec']}"))
1453
+ else:
1454
+ # Generic checks (no frame size or small frame)
1455
+ if pos_p is not None and isinstance(pos_p, (int, float)):
1456
+ if pos_p > 50:
1457
+ findings.append(Finding(
1458
+ WARNING, "Navigation", f"Position hold P = {pos_p} - aggressive",
1459
+ "High position P gain can cause oscillation in position hold and RTH. "
1460
+ "The quad overcorrects, overshoots, and oscillates around the target position.",
1461
+ setting="nav_mc_pos_xy_p",
1462
+ current=str(pos_p),
1463
+ recommended="20-35",
1464
+ cli_fix=f"set nav_mc_pos_xy_p = 30"))
1465
+ elif pos_p < 15:
1466
+ findings.append(Finding(
1467
+ INFO, "Navigation", f"Position hold P = {pos_p} - conservative",
1468
+ "Low position P may result in slow corrections and drifting in wind.",
1469
+ setting="nav_mc_pos_xy_p",
1470
+ current=str(pos_p)))
1471
+
1472
+ if heading_p is not None and isinstance(heading_p, (int, float)):
1473
+ if heading_p > 60:
1474
+ findings.append(Finding(
1475
+ INFO, "Navigation", f"Nav heading P = {heading_p} - high",
1476
+ "High heading P during navigation can cause yaw oscillation. "
1477
+ "Default is 60, consider lowering if you see heading wobble during RTH.",
1478
+ setting="nav_mc_heading_p",
1479
+ current=str(heading_p)))
1480
+
1481
+ return findings
1482
+
1483
+
1484
+ # ─── GPS Checks ──────────────────────────────────────────────────────────────
1485
+
1486
+ def check_gps(parsed):
1487
+ findings = []
1488
+
1489
+ has_gps = "GPS" in parsed["features"]
1490
+ if not has_gps:
1491
+ if any(get_setting(parsed, k) for k in ["nav_rth_altitude", "failsafe_procedure"]
1492
+ if get_setting(parsed, k) in ("RTH", 2)):
1493
+ findings.append(Finding(
1494
+ CRITICAL, "GPS", "GPS feature not enabled but RTH failsafe configured",
1495
+ "Failsafe is set to RTH but GPS is not enabled. The quad cannot navigate home.",
1496
+ setting="feature GPS",
1497
+ current="disabled",
1498
+ recommended="enabled",
1499
+ cli_fix="feature GPS"))
1500
+ return findings
1501
+
1502
+ findings.append(Finding(
1503
+ OK, "GPS", "GPS enabled",
1504
+ "GPS feature is active.",
1505
+ setting="feature GPS"))
1506
+
1507
+ # Multi-constellation
1508
+ galileo = get_setting(parsed, "gps_ublox_use_galileo", False)
1509
+ beidou = get_setting(parsed, "gps_ublox_use_beidou", False)
1510
+ glonass = get_setting(parsed, "gps_ublox_use_glonass", False)
1511
+
1512
+ constellations = ["GPS"] # always on
1513
+ if galileo: constellations.append("Galileo")
1514
+ if beidou: constellations.append("BeiDou")
1515
+ if glonass: constellations.append("GLONASS")
1516
+
1517
+ if len(constellations) >= 3:
1518
+ findings.append(Finding(
1519
+ OK, "GPS", f"Multi-constellation: {', '.join(constellations)}",
1520
+ "Multiple GNSS constellations improve fix quality and satellite count.",
1521
+ setting="gps_ublox_use_*"))
1522
+ elif len(constellations) == 1:
1523
+ findings.append(Finding(
1524
+ INFO, "GPS", "Only GPS constellation enabled",
1525
+ "Enabling Galileo and/or GLONASS improves fix quality with more visible satellites.",
1526
+ setting="gps_ublox_use_galileo",
1527
+ recommended="ON",
1528
+ cli_fix="set gps_ublox_use_galileo = ON"))
1529
+
1530
+ # Compass
1531
+ mag_hw = get_setting(parsed, "mag_hardware", None)
1532
+ align_mag = get_setting(parsed, "align_mag", "DEFAULT")
1533
+ if mag_hw and str(mag_hw).upper() not in ("NONE", "0", "FALSE"):
1534
+ findings.append(Finding(
1535
+ OK, "GPS", f"Compass: {mag_hw} (alignment: {align_mag})",
1536
+ "Compass is configured. Critical for accurate heading in GPS modes.",
1537
+ setting="mag_hardware"))
1538
+
1539
+ # Compass calibration quality check
1540
+ mag_gains = []
1541
+ for axis in ("x", "y", "z"):
1542
+ g = get_setting(parsed, f"maggain_{axis}", None)
1543
+ if g is not None and isinstance(g, (int, float)):
1544
+ mag_gains.append(g)
1545
+ if len(mag_gains) == 3:
1546
+ spread = (max(mag_gains) - min(mag_gains)) / max(max(mag_gains), 1) * 100
1547
+ if spread > 40:
1548
+ findings.append(Finding(
1549
+ WARNING, "GPS",
1550
+ f"Compass calibration may be poor (gain spread: {spread:.0f}%)",
1551
+ f"Mag gains: X={mag_gains[0]}, Y={mag_gains[1]}, Z={mag_gains[2]}. "
1552
+ f"A spread above 30-40% suggests the compass calibration was done near "
1553
+ f"metallic objects or with motor interference. Poor compass cal causes "
1554
+ f"toilet-bowling in position hold and erratic RTH heading. "
1555
+ f"Recalibrate outdoors, away from metal, with motors off.",
1556
+ setting="maggain_x/y/z",
1557
+ current=f"X={mag_gains[0]} Y={mag_gains[1]} Z={mag_gains[2]}",
1558
+ recommended="Recalibrate compass outdoors"))
1559
+ elif spread > 25:
1560
+ findings.append(Finding(
1561
+ INFO, "GPS",
1562
+ f"Compass gain spread: {spread:.0f}% - borderline",
1563
+ f"Mag gains: X={mag_gains[0]}, Y={mag_gains[1]}, Z={mag_gains[2]}. "
1564
+ f"This is borderline acceptable. If you see heading drift in poshold "
1565
+ f"or toilet-bowling, recalibrate the compass.",
1566
+ setting="maggain_x/y/z"))
1567
+
1568
+ elif has_gps:
1569
+ findings.append(Finding(
1570
+ WARNING, "GPS", "No compass configured",
1571
+ "GPS navigation without a compass relies on GPS heading, which only works "
1572
+ "while moving. Hover position hold and slow-speed RTH will have poor heading accuracy.",
1573
+ setting="mag_hardware",
1574
+ recommended="Install and configure a compass"))
1575
+
1576
+ return findings
1577
+
1578
+
1579
+ # ─── Blackbox Checks ─────────────────────────────────────────────────────────
1580
+
1581
+ def check_blackbox(parsed):
1582
+ findings = []
1583
+
1584
+ bb_denom = get_setting(parsed, "blackbox_rate_denom", 1)
1585
+ if isinstance(bb_denom, (int, float)) and bb_denom > 1:
1586
+ d = int(bb_denom)
1587
+ rate_desc = "half" if d == 2 else f"1/{d}"
1588
+ findings.append(Finding(
1589
+ INFO, "Blackbox", f"Blackbox logging at {rate_desc} rate",
1590
+ f"Recording every {d}{'nd' if d==2 else 'rd' if d==3 else 'th'} sample. "
1591
+ f"This saves flash space but reduces analysis resolution. "
1592
+ f"For PID tuning, 1/1 (full rate) gives the best data. "
1593
+ f"For long flights, 1/2 is a reasonable compromise.",
1594
+ setting="blackbox_rate_denom",
1595
+ current=str(bb_denom),
1596
+ recommended="1 for tuning, 2 for long flights"))
1597
+
1598
+ # Check if essential fields are being logged
1599
+ disabled = parsed["blackbox_disabled"]
1600
+ essential_for_tuning = ["GYRO_RAW", "MOTORS", "RC_COMMAND"]
1601
+ missing = [f for f in essential_for_tuning if f in disabled]
1602
+ if missing:
1603
+ findings.append(Finding(
1604
+ WARNING, "Blackbox", f"Essential blackbox fields disabled: {', '.join(missing)}",
1605
+ "These fields are needed for PID tuning analysis. "
1606
+ "Without them, the blackbox analyzer cannot give accurate recommendations.",
1607
+ setting="blackbox",
1608
+ current=f"{', '.join(missing)} disabled",
1609
+ recommended="Enable all for tuning",
1610
+ cli_fix="\n".join(f"blackbox {f}" for f in missing)))
1611
+
1612
+ # Navigation fields for nav analysis
1613
+ nav_disabled = [f for f in ["NAV_ACC", "NAV_POS"] if f in disabled]
1614
+ if nav_disabled:
1615
+ findings.append(Finding(
1616
+ INFO, "Blackbox", f"Nav blackbox fields disabled: {', '.join(nav_disabled)}",
1617
+ "These fields help analyze navigation performance (position hold, RTH). "
1618
+ "Enable them if you want to analyze nav tuning.",
1619
+ setting="blackbox",
1620
+ current=f"{', '.join(nav_disabled)} disabled"))
1621
+
1622
+ return findings
1623
+
1624
+
1625
+ # ─── Battery Checks ──────────────────────────────────────────────────────────
1626
+
1627
+ def check_battery(parsed):
1628
+ findings = []
1629
+ battery = get_active_battery(parsed)
1630
+
1631
+ min_cell = battery.get("vbat_min_cell_voltage",
1632
+ get_setting(parsed, "vbat_min_cell_voltage", 330))
1633
+ warn_cell = battery.get("vbat_warning_cell_voltage",
1634
+ get_setting(parsed, "vbat_warning_cell_voltage", 350))
1635
+ capacity = battery.get("battery_capacity", get_setting(parsed, "battery_capacity", 0))
1636
+ cap_warn = battery.get("battery_capacity_warning",
1637
+ get_setting(parsed, "battery_capacity_warning", 0))
1638
+ cap_crit = battery.get("battery_capacity_critical",
1639
+ get_setting(parsed, "battery_capacity_critical", 0))
1640
+
1641
+ if isinstance(min_cell, (int, float)):
1642
+ min_v = min_cell / 100 if min_cell > 100 else min_cell
1643
+ # Heuristic: large capacity (>7000mAh) + low min voltage often = Li-ion pack
1644
+ likely_lion = (isinstance(capacity, (int, float)) and capacity >= 7000
1645
+ and min_v < 3.0)
1646
+ if min_v < 3.0:
1647
+ if likely_lion:
1648
+ findings.append(Finding(
1649
+ INFO, "Battery", f"Minimum cell voltage: {min_v:.2f}V (likely Li-ion)",
1650
+ f"With {capacity}mAh capacity and {min_v:.2f}V minimum, this looks like a Li-ion "
1651
+ f"setup (18650/21700). This voltage is appropriate for Li-ion. "
1652
+ f"If you switch to LiPo, raise this to 3.3V.",
1653
+ setting="vbat_min_cell_voltage",
1654
+ current=f"{min_v:.2f}V"))
1655
+ else:
1656
+ findings.append(Finding(
1657
+ WARNING, "Battery", f"Minimum cell voltage: {min_v:.2f}V - low for LiPo",
1658
+ "If using LiPo batteries, going below 3.0V per cell causes permanent damage. "
1659
+ "3.3V is the recommended minimum for LiPo. If you're running Li-ion cells "
1660
+ "(18650/21700), 2.5-2.8V is normal - you can ignore this warning.",
1661
+ setting="vbat_min_cell_voltage",
1662
+ current=f"{min_v:.2f}V",
1663
+ recommended="330 (3.30V) for LiPo, 270 (2.70V) for Li-ion",
1664
+ cli_fix="set vbat_min_cell_voltage = 330"))
1665
+ elif min_v < 3.2:
1666
+ findings.append(Finding(
1667
+ INFO, "Battery", f"Minimum cell voltage: {min_v:.2f}V - low side",
1668
+ "Below 3.2V accelerates LiPo wear. 3.3-3.5V is safer.",
1669
+ setting="vbat_min_cell_voltage",
1670
+ current=f"{min_v:.2f}V",
1671
+ recommended="330-350"))
1672
+
1673
+ if isinstance(capacity, (int, float)) and capacity > 0:
1674
+ findings.append(Finding(
1675
+ OK, "Battery", f"Battery capacity: {capacity}mAh",
1676
+ "Capacity-based monitoring is configured.",
1677
+ setting="battery_capacity",
1678
+ current=f"{capacity}mAh"))
1679
+
1680
+ if isinstance(cap_warn, (int, float)) and cap_warn > 0:
1681
+ warn_pct = cap_warn / capacity * 100
1682
+ findings.append(Finding(
1683
+ OK if warn_pct >= 30 else INFO, "Battery",
1684
+ f"Warning at {cap_warn}mAh remaining ({warn_pct:.0f}%)",
1685
+ f"Battery warning triggers at {warn_pct:.0f}% remaining capacity.",
1686
+ setting="battery_capacity_warning"))
1687
+ elif capacity == 0:
1688
+ findings.append(Finding(
1689
+ INFO, "Battery", "No battery capacity set",
1690
+ "Capacity-based monitoring gives more accurate remaining flight time "
1691
+ "than voltage alone. Set battery_capacity to your pack's mAh rating.",
1692
+ setting="battery_capacity",
1693
+ recommended="Set to your battery mAh"))
1694
+
1695
+ return findings
1696
+
1697
+
1698
+ # ─── Receiver Checks ─────────────────────────────────────────────────────────
1699
+
1700
+ def check_rx(parsed):
1701
+ findings = []
1702
+
1703
+ rx_provider = get_setting(parsed, "serialrx_provider", None)
1704
+ if rx_provider:
1705
+ findings.append(Finding(
1706
+ OK, "Receiver", f"Serial RX: {rx_provider}",
1707
+ f"Using {rx_provider} protocol.",
1708
+ setting="serialrx_provider",
1709
+ current=str(rx_provider)))
1710
+
1711
+ channel_map = parsed["master"].get("channel_map", "AETR")
1712
+ if channel_map != "AETR" and channel_map != "TAER":
1713
+ findings.append(Finding(
1714
+ INFO, "Receiver", f"Channel map: {channel_map}",
1715
+ "Non-standard channel map. Make sure your transmitter matches.",
1716
+ setting="map",
1717
+ current=channel_map))
1718
+
1719
+ return findings
1720
+
1721
+
1722
+ # ─── General Checks ──────────────────────────────────────────────────────────
1723
+
1724
+ def check_general(parsed):
1725
+ findings = []
1726
+
1727
+ # Craft name
1728
+ name = get_setting(parsed, "name", "")
1729
+ if not name:
1730
+ findings.append(Finding(
1731
+ INFO, "General", "No craft name set",
1732
+ "Setting a name helps identify logs and OSD display.",
1733
+ setting="name",
1734
+ cli_fix='set name = MY_QUAD'))
1735
+
1736
+ # I2C speed
1737
+ i2c = get_setting(parsed, "i2c_speed", "400KHZ")
1738
+ if str(i2c).upper() == "800KHZ":
1739
+ findings.append(Finding(
1740
+ OK, "General", "I2C bus at 800KHz",
1741
+ "Fast I2C reduces sensor read latency.",
1742
+ setting="i2c_speed",
1743
+ current="800KHZ"))
1744
+
1745
+ # Airmode
1746
+ airmode = get_setting(parsed, "airmode_type", "STICK_CENTER")
1747
+ if str(airmode).upper() == "THROTTLE_THRESHOLD":
1748
+ findings.append(Finding(
1749
+ OK, "General", "Airmode: throttle threshold",
1750
+ "Airmode activates above throttle threshold - good for preventing accidental "
1751
+ "motor spin on the ground.",
1752
+ setting="airmode_type",
1753
+ current="THROTTLE_THRESHOLD"))
1754
+
1755
+ return findings
1756
+
1757
+
1758
+ # ─── Blackbox Cross-Reference ────────────────────────────────────────────────
1759
+
1760
+ def check_crossref_blackbox(parsed, bb_state):
1761
+ """Cross-reference config with blackbox analysis results."""
1762
+ findings = []
1763
+
1764
+ if not isinstance(bb_state, dict):
1765
+ return findings
1766
+
1767
+ actions = bb_state.get("actions", [])
1768
+ config = bb_state.get("config", {})
1769
+ scores = bb_state.get("scores", {})
1770
+
1771
+ overall = scores.get("overall", 0)
1772
+ noise_score = scores.get("noise")
1773
+ pid_score = scores.get("pid")
1774
+
1775
+ if overall > 0:
1776
+ findings.append(Finding(
1777
+ OK if overall >= 75 else WARNING if overall >= 50 else CRITICAL,
1778
+ "Blackbox", f"Last tune quality: {overall:.0f}/100",
1779
+ f"Noise: {noise_score:.0f}, PID: {pid_score:.0f}, Motors: {scores.get('motor', 0):.0f}" if noise_score else "",
1780
+ current=f"{overall:.0f}/100"))
1781
+
1782
+ # Check if EZ Tune is fighting manual PID changes
1783
+ ez_active = get_setting(parsed, "ez_enabled", None) is True
1784
+ if ez_active:
1785
+ bb_roll_p = config.get("roll_p")
1786
+ diff_roll_p = get_active_control(parsed).get("mc_p_roll")
1787
+ if bb_roll_p and diff_roll_p and bb_roll_p != diff_roll_p:
1788
+ findings.append(Finding(
1789
+ WARNING, "Blackbox", "PID values differ between diff and blackbox",
1790
+ f"Roll P in diff: {diff_roll_p}, in last blackbox: {bb_roll_p}. "
1791
+ f"With EZ Tune active, manually set PIDs will be overwritten on reboot. "
1792
+ f"Either tune through EZ Tune parameters or disable EZ Tune (set ez_enabled = OFF).",
1793
+ setting="ez_enabled"))
1794
+
1795
+ return findings
1796
+
1797
+
1798
+ # ─── Terminal Output ─────────────────────────────────────────────────────────
1799
+
1800
+ # ─── Pre-Flight Sanity Check ─────────────────────────────────────────────────
1801
+ # A "will this crash on first flight?" validator.
1802
+ # Different from the tuning-focused analysis above — this catches
1803
+ # dangerous misconfigurations that can destroy hardware or hurt people.
1804
+
1805
+ class SanityItem:
1806
+ """A single pre-flight check result."""
1807
+ FAIL = "FAIL"
1808
+ WARN = "WARN"
1809
+ ASK = "ASK"
1810
+ PASS = "PASS"
1811
+
1812
+ def __init__(self, status, category, message, detail=None, question=None,
1813
+ recommendation=None, cli_fix=None):
1814
+ self.status = status
1815
+ self.category = category
1816
+ self.message = message
1817
+ self.detail = detail or ""
1818
+ self.question = question # Interactive confirmation prompt
1819
+ self.recommendation = recommendation
1820
+ self.cli_fix = cli_fix
1821
+ self.user_confirmed = None # None=not asked, True/False=answer
1822
+
1823
+ def __repr__(self):
1824
+ return f"<{self.status} {self.category}: {self.message}>"
1825
+
1826
+
1827
+ def _ask_pilot(question, default_yes=False):
1828
+ """Ask the pilot a yes/no question interactively."""
1829
+ suffix = "[Y/n]" if default_yes else "[y/N]"
1830
+ try:
1831
+ answer = input(f" ? {question} {suffix} ").strip().lower()
1832
+ if not answer:
1833
+ return default_yes
1834
+ return answer in ("y", "yes")
1835
+ except (EOFError, KeyboardInterrupt):
1836
+ print()
1837
+ return default_yes
1838
+
1839
+
1840
+ def _get_assigned_modes(parsed):
1841
+ """Return set of mode IDs that have aux channel assignments."""
1842
+ return {m["mode_id"] for m in parsed["aux_modes"]
1843
+ if m["range_low"] < m["range_high"]}
1844
+
1845
+
1846
+ def _has_gps_uart(parsed):
1847
+ """Check if any serial port has GPS function enabled."""
1848
+ for _port_id, config in parsed["serial_ports"].items():
1849
+ parts = config.split()
1850
+ if len(parts) >= 1:
1851
+ try:
1852
+ func_mask = int(parts[0])
1853
+ if func_mask & 2: # GPS function bit
1854
+ return True
1855
+ except ValueError:
1856
+ pass
1857
+ return False
1858
+
1859
+
1860
+ def _has_feature(parsed, feat_name):
1861
+ """Check if a feature is enabled (present in features list)."""
1862
+ return feat_name.upper() in [f.upper() for f in parsed["features"]]
1863
+
1864
+
1865
+ def _has_feature_disabled(parsed, feat_name):
1866
+ """Check if a feature is explicitly disabled."""
1867
+ return feat_name.upper() in [f.upper() for f in parsed.get("features_disabled", [])]
1868
+
1869
+
1870
+ def _detect_frame_from_name(parsed):
1871
+ """Try to detect frame size from craft name."""
1872
+ name = get_setting(parsed, "name", "")
1873
+ if not name:
1874
+ return None
1875
+ name_upper = str(name).upper()
1876
+ for size in [15, 12, 10, 7, 5]:
1877
+ if str(size) in name_upper:
1878
+ return size
1879
+ return None
1880
+
1881
+
1882
+ def _detect_motor_count(parsed):
1883
+ """Detect motor count from motor_mix or platform type."""
1884
+ if parsed["motor_mix"]:
1885
+ return max(m["index"] for m in parsed["motor_mix"]) + 1
1886
+ # Infer from platform_type
1887
+ platform = get_setting(parsed, "platform_type", "")
1888
+ platform_map = {
1889
+ "QUADX": 4, "QUAD": 4, "QUADP": 4,
1890
+ "HEX6": 6, "HEXX": 6, "HEXH": 6, "HEX6X": 6,
1891
+ "OCTOX8": 8, "OCTOFLATX": 8, "OCTOFLATP": 8,
1892
+ "Y6": 6,
1893
+ "TRI": 3, "TRICOPTER": 3,
1894
+ }
1895
+ mixer = get_setting(parsed, "mixer_preset", "")
1896
+ if isinstance(mixer, str):
1897
+ for key, count in platform_map.items():
1898
+ if key in mixer.upper():
1899
+ return count
1900
+ return None
1901
+
1902
+
1903
+ def run_sanity_check(parsed, frame_inches=None, interactive=True):
1904
+ """Run pre-flight sanity checks. Returns list of SanityItems."""
1905
+ items = []
1906
+
1907
+ # Auto-detect frame size if not provided
1908
+ if frame_inches is None:
1909
+ frame_inches = _detect_frame_from_name(parsed)
1910
+
1911
+ assigned_modes = _get_assigned_modes(parsed)
1912
+ has_gps = _has_gps_uart(parsed)
1913
+ motor_count = _detect_motor_count(parsed)
1914
+ profile = get_active_control(parsed)
1915
+
1916
+ # ── 1. ARMING ────────────────────────────────────────────────────────
1917
+ if 0 not in assigned_modes:
1918
+ items.append(SanityItem(
1919
+ SanityItem.FAIL, "Arming",
1920
+ "No ARM switch assigned",
1921
+ "Without an ARM mode on an aux channel, the aircraft cannot be armed "
1922
+ "(or worse, may arm unexpectedly if using stick commands).",
1923
+ recommendation="Assign ARM to an AUX channel in the Modes tab"))
1924
+ else:
1925
+ arm_modes = [m for m in parsed["aux_modes"] if m["mode_id"] == 0]
1926
+ for am in arm_modes:
1927
+ rng = am["range_high"] - am["range_low"]
1928
+ if rng > 800:
1929
+ items.append(SanityItem(
1930
+ SanityItem.WARN, "Arming",
1931
+ f"ARM range is very wide ({am['range_low']}-{am['range_high']})",
1932
+ "A wide ARM range increases the risk of accidental arming.",
1933
+ recommendation="Use a narrow range like 1800-2100"))
1934
+ else:
1935
+ items.append(SanityItem(
1936
+ SanityItem.PASS, "Arming",
1937
+ "ARM switch assigned"))
1938
+
1939
+ # ── 2. MOTOR OUTPUT ──────────────────────────────────────────────────
1940
+ output_enabled = get_setting(parsed, "motor_pwm_protocol", "")
1941
+ # In INAV, motor output is disabled by default until you enable it in Outputs tab.
1942
+ # The feature MOTOR_STOP / output is controlled differently.
1943
+ # If we see no motor protocol at all or DISABLED, that's a problem.
1944
+ if output_enabled == "DISABLED" or output_enabled == "" or output_enabled == 0:
1945
+ items.append(SanityItem(
1946
+ SanityItem.FAIL, "Motor Output",
1947
+ "Motor output appears DISABLED",
1948
+ "INAV disables motor output by default as a safety measure. "
1949
+ "Motors will not spin until you enable output in the Outputs tab.",
1950
+ recommendation="In Configurator: Outputs tab → enable motor output, select DSHOT protocol"))
1951
+
1952
+ # ── 3. MOTOR DIRECTION ───────────────────────────────────────────────
1953
+ motor_inverted = get_setting(parsed, "motor_direction_inverted", False)
1954
+ if motor_inverted:
1955
+ items.append(SanityItem(
1956
+ SanityItem.ASK, "Motors",
1957
+ "Motor direction is INVERTED (props-out configuration)",
1958
+ "motor_direction_inverted = ON means the FC expects reversed motor spin "
1959
+ "(props-out). If your motors spin the normal way (props-in), the quad "
1960
+ "will yaw violently and flip on takeoff.",
1961
+ question="Are you running props-out (reversed) motor direction?",
1962
+ recommendation="If props-in, set motor_direction_inverted = OFF",
1963
+ cli_fix="set motor_direction_inverted = OFF"))
1964
+ else:
1965
+ items.append(SanityItem(
1966
+ SanityItem.PASS, "Motors",
1967
+ "Motor direction: standard (props-in)"))
1968
+
1969
+ # ── 4. FAILSAFE ──────────────────────────────────────────────────────
1970
+ fs_proc = get_setting(parsed, "failsafe_procedure", "DROP")
1971
+ if fs_proc == "DROP" or fs_proc == 0:
1972
+ items.append(SanityItem(
1973
+ SanityItem.FAIL, "Failsafe",
1974
+ "Failsafe procedure is DROP",
1975
+ "On signal loss, the aircraft will disarm and freefall from any altitude. "
1976
+ "This is the default but extremely dangerous for any altitude flight.",
1977
+ recommendation="Set failsafe to RTH if you have GPS, or LAND",
1978
+ cli_fix="set failsafe_procedure = RTH"))
1979
+ elif (fs_proc == "RTH" or fs_proc == 2) and not has_gps:
1980
+ items.append(SanityItem(
1981
+ SanityItem.FAIL, "Failsafe",
1982
+ "Failsafe set to RTH but no GPS configured",
1983
+ "RTH failsafe requires GPS. Without it, the aircraft will fall back "
1984
+ "to emergency landing which may not work correctly.",
1985
+ recommendation="Configure GPS on a UART, or change failsafe to LAND"))
1986
+ elif fs_proc == "RTH" or fs_proc == 2:
1987
+ items.append(SanityItem(
1988
+ SanityItem.PASS, "Failsafe",
1989
+ "Failsafe set to RTH with GPS available"))
1990
+
1991
+ # Failsafe throttle + airmode
1992
+ airmode = get_setting(parsed, "airmode_type", "")
1993
+ fs_throttle = get_setting(parsed, "failsafe_throttle", 1000)
1994
+ if isinstance(fs_throttle, (int, float)) and fs_throttle <= 1050:
1995
+ if airmode and str(airmode).upper() != "STICK_CENTER":
1996
+ items.append(SanityItem(
1997
+ SanityItem.WARN, "Failsafe",
1998
+ f"Low failsafe throttle ({fs_throttle}) with airmode enabled",
1999
+ "Airmode keeps PID corrections active even at zero throttle. "
2000
+ "Combined with low failsafe throttle, this can cause the aircraft "
2001
+ "to tumble during emergency landing instead of descending smoothly.",
2002
+ recommendation="Consider raising failsafe_throttle to hover level"))
2003
+
2004
+ # ── 5. BATTERY ───────────────────────────────────────────────────────
2005
+ batt_profile = get_active_battery(parsed)
2006
+ vmin = batt_profile.get("bat_voltage_cell_min", get_setting(parsed, "bat_voltage_cell_min", 330))
2007
+ vmax = batt_profile.get("bat_voltage_cell_max", get_setting(parsed, "bat_voltage_cell_max", 420))
2008
+ vwarn = batt_profile.get("bat_voltage_cell_warning", get_setting(parsed, "bat_voltage_cell_warning", 350))
2009
+
2010
+ if isinstance(vmin, (int, float)) and isinstance(vmax, (int, float)):
2011
+ if vmin >= vmax:
2012
+ items.append(SanityItem(
2013
+ SanityItem.FAIL, "Battery",
2014
+ f"Cell min voltage ({vmin/100:.2f}V) >= max ({vmax/100:.2f}V)",
2015
+ "Battery voltage limits are inverted or equal. "
2016
+ "The aircraft will think the battery is always critical."))
2017
+ elif vmin < 250 or vmin > 380:
2018
+ items.append(SanityItem(
2019
+ SanityItem.WARN, "Battery",
2020
+ f"Unusual cell min voltage: {vmin/100:.2f}V",
2021
+ "Normal range is 2.80-3.50V for LiPo, 2.50-2.80V for Li-ion.",
2022
+ question="Is this voltage correct for your battery type?"))
2023
+ else:
2024
+ items.append(SanityItem(
2025
+ SanityItem.PASS, "Battery",
2026
+ f"Battery limits: {vmin/100:.2f}V min, {vmax/100:.2f}V max"))
2027
+
2028
+ # No battery monitoring
2029
+ if _has_feature_disabled(parsed, "VBAT"):
2030
+ items.append(SanityItem(
2031
+ SanityItem.FAIL, "Battery",
2032
+ "Battery voltage monitoring is DISABLED",
2033
+ "Without voltage monitoring, you will have no low battery warning "
2034
+ "and no battery-related failsafe. The aircraft will fly until "
2035
+ "the battery cuts out, causing an uncontrolled crash.",
2036
+ recommendation="Enable VBAT feature",
2037
+ cli_fix="feature VBAT"))
2038
+
2039
+ # ── 6. GPS & NAVIGATION ──────────────────────────────────────────────
2040
+ nav_modes_assigned = assigned_modes & GPS_MODES
2041
+ nav_mode_names = [INAV_MODES.get(m, f"Mode {m}") for m in nav_modes_assigned]
2042
+
2043
+ if nav_modes_assigned and not has_gps:
2044
+ items.append(SanityItem(
2045
+ SanityItem.FAIL, "Navigation",
2046
+ f"Nav modes assigned but no GPS configured: {', '.join(nav_mode_names)}",
2047
+ "These modes require GPS to function. Without GPS, activating them "
2048
+ "will cause unpredictable behavior or no effect at all.",
2049
+ recommendation="Configure GPS on a UART in the Ports tab"))
2050
+ elif nav_modes_assigned and has_gps:
2051
+ items.append(SanityItem(
2052
+ SanityItem.PASS, "Navigation",
2053
+ f"GPS configured for nav modes: {', '.join(nav_mode_names)}"))
2054
+
2055
+ if has_gps and not nav_modes_assigned and 11 not in assigned_modes:
2056
+ items.append(SanityItem(
2057
+ SanityItem.WARN, "Navigation",
2058
+ "GPS configured but no NAV RTH mode assigned",
2059
+ "You have GPS hardware configured but no return-to-home switch. "
2060
+ "RTH is valuable as an emergency recovery option.",
2061
+ recommendation="Assign NAV RTH to an AUX channel"))
2062
+
2063
+ # ALTHOLD without baro
2064
+ alt_modes_assigned = assigned_modes & ALT_MODES
2065
+ # Check for baro - it's usually a hardware feature, not always in diff
2066
+ baro_hardware = get_setting(parsed, "baro_hardware", "")
2067
+ if alt_modes_assigned and baro_hardware and str(baro_hardware).upper() == "NONE":
2068
+ items.append(SanityItem(
2069
+ SanityItem.FAIL, "Navigation",
2070
+ "NAV ALTHOLD assigned but barometer is disabled",
2071
+ "Altitude hold requires a barometer. Without it, altitude control "
2072
+ "will not work and the aircraft may climb or descend uncontrollably.",
2073
+ recommendation="Enable barometer hardware or remove ALTHOLD mode"))
2074
+
2075
+ # POSHOLD/RTH without compass
2076
+ compass_modes_assigned = assigned_modes & COMPASS_MODES
2077
+ compass_mode_names = [INAV_MODES.get(m, f"Mode {m}") for m in compass_modes_assigned]
2078
+ mag_hardware = get_setting(parsed, "mag_hardware", "")
2079
+ mag_disabled = (isinstance(mag_hardware, str) and mag_hardware.upper() == "NONE") or mag_hardware == 0
2080
+ if compass_modes_assigned and mag_disabled:
2081
+ items.append(SanityItem(
2082
+ SanityItem.WARN, "Navigation",
2083
+ f"Compass disabled but nav modes assigned: {', '.join(compass_mode_names)}",
2084
+ "INAV can fly nav modes without compass using GPS-derived heading, "
2085
+ "but position hold performance is reduced and toilet-bowl patterns "
2086
+ "are more likely, especially at low speed.",
2087
+ question="Are you intentionally flying without compass?"))
2088
+
2089
+ # Hover throttle at default
2090
+ hover_thr = get_setting(parsed, "nav_mc_hover_thr", 1500)
2091
+ if isinstance(hover_thr, (int, float)) and hover_thr == 1500:
2092
+ if nav_modes_assigned:
2093
+ items.append(SanityItem(
2094
+ SanityItem.ASK, "Navigation",
2095
+ "Hover throttle is at default (1500)",
2096
+ "The default hover throttle may not match your aircraft. "
2097
+ "If too low, the aircraft will sink in altitude hold. "
2098
+ "If too high, it will climb.",
2099
+ question="Have you calibrated the hover throttle for this aircraft?",
2100
+ recommendation="Hover the aircraft, note the throttle value, "
2101
+ "set nav_mc_hover_thr to that value"))
2102
+
2103
+ # ── 7. PIDS ──────────────────────────────────────────────────────────
2104
+ p_roll = get_setting(parsed, "mc_p_roll", profile.get("mc_p_roll", 40))
2105
+ p_pitch = get_setting(parsed, "mc_p_pitch", profile.get("mc_p_pitch", 44))
2106
+ d_roll = get_setting(parsed, "mc_d_roll", profile.get("mc_d_roll", 25))
2107
+ d_pitch = get_setting(parsed, "mc_d_pitch", profile.get("mc_d_pitch", 25))
2108
+
2109
+ # Zero P = no stabilization
2110
+ if isinstance(p_roll, (int, float)) and isinstance(p_pitch, (int, float)):
2111
+ if p_roll == 0 or p_pitch == 0:
2112
+ items.append(SanityItem(
2113
+ SanityItem.FAIL, "PIDs",
2114
+ f"P-term is ZERO (roll={p_roll}, pitch={p_pitch})",
2115
+ "With P=0 the aircraft has no stabilization on that axis. "
2116
+ "It will be completely uncontrollable.",
2117
+ recommendation="Set P values to at least 20"))
2118
+
2119
+ # Extremely high PIDs
2120
+ if isinstance(p_roll, (int, float)) and isinstance(p_pitch, (int, float)):
2121
+ if p_roll > 100 or p_pitch > 100:
2122
+ items.append(SanityItem(
2123
+ SanityItem.FAIL, "PIDs",
2124
+ f"P-term is extremely high (roll={p_roll}, pitch={p_pitch})",
2125
+ "P values above 100 will cause violent oscillation on any airframe. "
2126
+ "The aircraft will shake itself apart on takeoff.",
2127
+ recommendation="Reduce P to a sane starting point for your frame size"))
2128
+
2129
+ # PID mismatch with detected frame size
2130
+ if frame_inches and frame_inches in FRAME_PROFILES:
2131
+ fp = FRAME_PROFILES[frame_inches]
2132
+ expected_p = fp["pids"].get("mc_p_roll", 40)
2133
+
2134
+ if isinstance(p_roll, (int, float)):
2135
+ # Check if PIDs are way off for this frame size
2136
+ # A 10" frame with 5" PIDs (40-44) is too aggressive
2137
+ # A 5" frame with 15" PIDs (26) is too sluggish
2138
+ if frame_inches >= 10 and p_roll >= 44:
2139
+ # Looks like 5"/7" defaults on a big frame
2140
+ items.append(SanityItem(
2141
+ SanityItem.ASK, "PIDs",
2142
+ f"PIDs look aggressive for a {frame_inches}-inch frame "
2143
+ f"(P_roll={p_roll}, expected ~{expected_p})",
2144
+ f"These PID values are typical for a 5-7 inch quad. On a "
2145
+ f"{frame_inches}-inch frame, this will likely cause violent "
2146
+ f"oscillation due to higher prop inertia.",
2147
+ question=f"Is this actually a {frame_inches}-inch frame?",
2148
+ recommendation=f"Use inav-params --setup {frame_inches} for starting PIDs",
2149
+ cli_fix=f"# Run: inav-params --setup {frame_inches}"))
2150
+ elif frame_inches <= 5 and p_roll < 25:
2151
+ items.append(SanityItem(
2152
+ SanityItem.ASK, "PIDs",
2153
+ f"PIDs look very low for a {frame_inches}-inch frame "
2154
+ f"(P_roll={p_roll}, expected ~{expected_p})",
2155
+ "Very low PIDs on a small frame will make it feel mushy and unresponsive.",
2156
+ question=f"Is this actually a {frame_inches}-inch frame?"))
2157
+
2158
+ if isinstance(p_roll, (int, float)) and isinstance(p_pitch, (int, float)):
2159
+ if not (p_roll == 0 or p_pitch == 0) and not (p_roll > 100 or p_pitch > 100):
2160
+ items.append(SanityItem(
2161
+ SanityItem.PASS, "PIDs",
2162
+ f"PIDs in reasonable range (P_roll={p_roll}, P_pitch={p_pitch})"))
2163
+
2164
+ # ── 8. RATES ─────────────────────────────────────────────────────────
2165
+ roll_rate = get_setting(parsed, "roll_rate",
2166
+ profile.get("roll_rate", 40))
2167
+ pitch_rate = get_setting(parsed, "pitch_rate",
2168
+ profile.get("pitch_rate", 40))
2169
+ yaw_rate = get_setting(parsed, "yaw_rate",
2170
+ profile.get("yaw_rate", 30))
2171
+
2172
+ for axis, rate in [("Roll", roll_rate), ("Pitch", pitch_rate), ("Yaw", yaw_rate)]:
2173
+ if isinstance(rate, (int, float)):
2174
+ if rate == 0:
2175
+ items.append(SanityItem(
2176
+ SanityItem.FAIL, "Rates",
2177
+ f"{axis} rate is ZERO",
2178
+ f"With rate=0, the aircraft cannot rotate on the {axis.lower()} axis. "
2179
+ "It will be uncontrollable.",
2180
+ recommendation=f"Set {axis.lower()}_rate to at least 20"))
2181
+ elif rate > 120: # 1200 dps — extreme
2182
+ items.append(SanityItem(
2183
+ SanityItem.WARN, "Rates",
2184
+ f"{axis} rate is extremely high ({rate} = {rate*10}dps)",
2185
+ "Very high rates make the aircraft extremely twitchy and hard to control. "
2186
+ "Most pilots use 200-700dps for multirotors.",
2187
+ question=f"Is {rate*10}dps {axis.lower()} rate intentional?"))
2188
+
2189
+ # ── 9. FILTERS ───────────────────────────────────────────────────────
2190
+ gyro_lpf = get_setting(parsed, "gyro_main_lpf_hz", 110)
2191
+ dterm_lpf = get_setting(parsed, "dterm_lpf_hz", 110)
2192
+ looptime = get_setting(parsed, "looptime", 500)
2193
+
2194
+ if isinstance(looptime, (int, float)) and looptime > 0:
2195
+ sample_rate = 1_000_000 / looptime
2196
+ nyquist = sample_rate / 2
2197
+
2198
+ if isinstance(gyro_lpf, (int, float)):
2199
+ if gyro_lpf == 0:
2200
+ items.append(SanityItem(
2201
+ SanityItem.WARN, "Filters",
2202
+ "Gyro LPF is DISABLED (0 Hz)",
2203
+ "Running with no gyro low-pass filter passes all noise directly "
2204
+ "to the PID controller. Unless you have very clean motors and "
2205
+ "a well-built frame, this will cause motor heating and oscillation.",
2206
+ question="Are you intentionally running without gyro LPF?"))
2207
+ elif gyro_lpf > nyquist:
2208
+ items.append(SanityItem(
2209
+ SanityItem.FAIL, "Filters",
2210
+ f"Gyro LPF ({gyro_lpf}Hz) is above Nyquist ({nyquist:.0f}Hz)",
2211
+ "The filter cutoff is higher than what the sample rate can represent. "
2212
+ "This provides no filtering and may cause aliasing artifacts.",
2213
+ recommendation=f"Lower gyro_main_lpf_hz below {nyquist:.0f}"))
2214
+
2215
+ if isinstance(dterm_lpf, (int, float)):
2216
+ if dterm_lpf == 0:
2217
+ items.append(SanityItem(
2218
+ SanityItem.WARN, "Filters",
2219
+ "D-term LPF is DISABLED (0 Hz)",
2220
+ "D-term amplifies noise by design. Without filtering, motor "
2221
+ "heating and high-frequency oscillation are very likely.",
2222
+ question="Are you intentionally running without D-term LPF?"))
2223
+
2224
+ if isinstance(gyro_lpf, (int, float)) and gyro_lpf > 0:
2225
+ if frame_inches and frame_inches in FRAME_PROFILES:
2226
+ expected_lpf = FRAME_PROFILES[frame_inches]["filters"]["gyro_main_lpf_hz"]
2227
+ if gyro_lpf > expected_lpf * 2:
2228
+ items.append(SanityItem(
2229
+ SanityItem.WARN, "Filters",
2230
+ f"Gyro LPF ({gyro_lpf}Hz) seems high for {frame_inches}-inch "
2231
+ f"(expected ~{expected_lpf}Hz)",
2232
+ "Higher filter cutoff passes more noise. Large frames need lower cutoffs "
2233
+ "because they vibrate at lower frequencies."))
2234
+ else:
2235
+ items.append(SanityItem(
2236
+ SanityItem.PASS, "Filters",
2237
+ f"Gyro LPF at {gyro_lpf}Hz"))
2238
+
2239
+ # ── 10. RECEIVER ─────────────────────────────────────────────────────
2240
+ rx_type = get_setting(parsed, "serialrx_provider", None)
2241
+ has_rx_uart = False
2242
+ for _port_id, config in parsed["serial_ports"].items():
2243
+ parts = config.split()
2244
+ if len(parts) >= 1:
2245
+ try:
2246
+ func_mask = int(parts[0])
2247
+ if func_mask & 64: # Serial RX function bit
2248
+ has_rx_uart = True
2249
+ except ValueError:
2250
+ pass
2251
+
2252
+ if not has_rx_uart and rx_type is not None:
2253
+ # Might be using SPI RX or other non-serial receiver
2254
+ items.append(SanityItem(
2255
+ SanityItem.PASS, "Receiver",
2256
+ f"Receiver protocol: {rx_type}"))
2257
+ elif has_rx_uart:
2258
+ items.append(SanityItem(
2259
+ SanityItem.PASS, "Receiver",
2260
+ f"Serial receiver configured" + (f" ({rx_type})" if rx_type else "")))
2261
+ else:
2262
+ items.append(SanityItem(
2263
+ SanityItem.WARN, "Receiver",
2264
+ "No serial receiver UART detected in diff",
2265
+ "This may be normal if using SPI RX (like on AIO boards) or "
2266
+ "if the receiver is on default UART2 (not shown in diff)."))
2267
+
2268
+ # ── 11. BOARD ALIGNMENT ──────────────────────────────────────────────
2269
+ align_roll = get_setting(parsed, "align_board_roll", 0)
2270
+ align_pitch = get_setting(parsed, "align_board_pitch", 0)
2271
+ align_yaw = get_setting(parsed, "align_board_yaw", 0)
2272
+
2273
+ has_alignment = False
2274
+ for axis_name, val in [("roll", align_roll), ("pitch", align_pitch), ("yaw", align_yaw)]:
2275
+ if isinstance(val, (int, float)) and val != 0:
2276
+ has_alignment = True
2277
+ degrees = val / 10.0 # INAV stores in decidegrees
2278
+ if abs(degrees) > 0 and abs(degrees) not in (90, 180, 270):
2279
+ items.append(SanityItem(
2280
+ SanityItem.ASK, "Board Alignment",
2281
+ f"Board alignment {axis_name} = {degrees:.1f} degrees",
2282
+ "Non-standard board alignment. Common values are 0, 90, 180, 270. "
2283
+ "Wrong alignment will cause the FC to fight you — stabilization "
2284
+ "will push the aircraft the wrong way.",
2285
+ question=f"Is your FC rotated {degrees:.1f} degrees on {axis_name}?"))
2286
+ elif abs(degrees) in (90, 180, 270):
2287
+ items.append(SanityItem(
2288
+ SanityItem.PASS, "Board Alignment",
2289
+ f"Board rotated {degrees:.0f}° on {axis_name}"))
2290
+
2291
+ if not has_alignment:
2292
+ items.append(SanityItem(
2293
+ SanityItem.PASS, "Board Alignment",
2294
+ "Standard orientation (no rotation)"))
2295
+
2296
+ # ── 12. OSD / VIDEO ──────────────────────────────────────────────────
2297
+ osd_video = get_setting(parsed, "osd_video_system", "")
2298
+ if isinstance(osd_video, str) and osd_video.upper() not in ("", "AUTO"):
2299
+ # Has specific video system configured
2300
+ items.append(SanityItem(
2301
+ SanityItem.PASS, "OSD",
2302
+ f"OSD video system: {osd_video}"))
2303
+
2304
+ # ── 13. BEEPER SAFETY ────────────────────────────────────────────────
2305
+ critical_beepers = ["BAT_CRIT_LOW", "BAT_LOW", "RX_LOST", "RX_LOST_LANDING"]
2306
+ disabled = parsed.get("beepers_disabled", [])
2307
+ missing_critical = [b for b in critical_beepers if b in disabled]
2308
+ if missing_critical:
2309
+ items.append(SanityItem(
2310
+ SanityItem.WARN, "Safety",
2311
+ f"Critical beepers disabled: {', '.join(missing_critical)}",
2312
+ "You will have no audible warning for low battery or signal loss.",
2313
+ recommendation="Enable critical beepers",
2314
+ cli_fix="\n".join(f"beeper {b}" for b in missing_critical)))
2315
+ else:
2316
+ items.append(SanityItem(
2317
+ SanityItem.PASS, "Safety",
2318
+ "Critical beepers enabled"))
2319
+
2320
+ # ── 14. TRICOPTER SERVO ──────────────────────────────────────────────
2321
+ platform = get_setting(parsed, "platform_type", "")
2322
+ if isinstance(platform, str) and "TRI" in platform.upper():
2323
+ # Check servo direction
2324
+ items.append(SanityItem(
2325
+ SanityItem.ASK, "Platform",
2326
+ "Tricopter detected — verify tail servo direction",
2327
+ "Tricopters need the tail servo to push in the correct direction "
2328
+ "for yaw authority. An inverted servo will cause uncontrollable yaw.",
2329
+ question="Have you verified the tail servo moves correctly for yaw "
2330
+ "in the Configurator Outputs tab?"))
2331
+
2332
+ # ── 15. HEX/OCTO MOTOR ORDER ────────────────────────────────────────
2333
+ if motor_count and motor_count >= 6:
2334
+ platform_name = "Hexacopter" if motor_count == 6 else "Octocopter"
2335
+ items.append(SanityItem(
2336
+ SanityItem.ASK, "Platform",
2337
+ f"{platform_name} detected ({motor_count} motors)",
2338
+ f"With {motor_count} motors, correct motor order and spin direction "
2339
+ "are critical. Verify in the Configurator Outputs tab that each motor "
2340
+ "matches the diagram.",
2341
+ question=f"Have you verified all {motor_count} motor positions and spin directions?"))
2342
+
2343
+ # ── 16. FLIGHT MODES ─────────────────────────────────────────────────
2344
+ has_stabilized = assigned_modes & {1, 2} # ANGLE or HORIZON
2345
+ if not has_stabilized and 12 not in assigned_modes:
2346
+ # No stabilized mode and no manual mode — only ARM
2347
+ items.append(SanityItem(
2348
+ SanityItem.WARN, "Modes",
2349
+ "No flight mode switch assigned (ANGLE/HORIZON)",
2350
+ "Without a flight mode on a switch, the aircraft defaults to "
2351
+ "ACRO/rate mode which requires advanced piloting skills. "
2352
+ "Most pilots want at least ANGLE mode for recovery.",
2353
+ question="Are you an experienced ACRO pilot?"))
2354
+ elif has_stabilized:
2355
+ mode_names = [INAV_MODES.get(m, "") for m in assigned_modes & {1, 2}]
2356
+ items.append(SanityItem(
2357
+ SanityItem.PASS, "Modes",
2358
+ f"Stabilized mode available: {', '.join(mode_names)}"))
2359
+
2360
+ return items
2361
+
2362
+
2363
+ def print_sanity_report(items, parsed, interactive=True):
2364
+ """Print the interactive pre-flight sanity check report."""
2365
+ R, B, C, G, Y, RED, DIM = _colors()
2366
+
2367
+ print(f"\n {B}{'=' * 55}{R}")
2368
+ print(f" {B} INAV Pre-Flight Sanity Check v{VERSION}{R}")
2369
+ print(f" {B}{'=' * 55}{R}")
2370
+
2371
+ # Aircraft identification
2372
+ name = get_setting(parsed, "name", "")
2373
+ board = parsed.get("board", "")
2374
+ version = parsed.get("version", "")
2375
+ platform = get_setting(parsed, "platform_type", "")
2376
+
2377
+ print(f"\n {B}AIRCRAFT{R}")
2378
+ if name:
2379
+ print(f" Craft name: {C}{name}{R}")
2380
+ if board:
2381
+ print(f" Board: {board}")
2382
+ if version:
2383
+ print(f" Firmware: INAV {version}")
2384
+ if platform:
2385
+ print(f" Platform: {platform}")
2386
+ print()
2387
+
2388
+ # Group items by category
2389
+ categories = []
2390
+ seen = set()
2391
+ for item in items:
2392
+ if item.category not in seen:
2393
+ categories.append(item.category)
2394
+ seen.add(item.category)
2395
+
2396
+ n_fail = 0
2397
+ n_warn = 0
2398
+ n_ask_unconfirmed = 0
2399
+ n_pass = 0
2400
+
2401
+ for category in categories:
2402
+ cat_items = [i for i in items if i.category == category]
2403
+ print(f" {B}{category}{R}")
2404
+
2405
+ for item in cat_items:
2406
+ if item.status == SanityItem.PASS:
2407
+ print(f" {G}✓{R} {item.message}")
2408
+ n_pass += 1
2409
+
2410
+ elif item.status == SanityItem.FAIL:
2411
+ print(f" {RED}✗ FAIL:{R} {item.message}")
2412
+ if item.detail:
2413
+ for line in textwrap.wrap(item.detail, 68):
2414
+ print(f" {DIM}{line}{R}")
2415
+ if item.recommendation:
2416
+ print(f" {Y}→ {item.recommendation}{R}")
2417
+ if item.cli_fix:
2418
+ print(f" {C}CLI: {item.cli_fix}{R}")
2419
+ n_fail += 1
2420
+
2421
+ elif item.status == SanityItem.WARN:
2422
+ print(f" {Y}⚠ WARNING:{R} {item.message}")
2423
+ if item.detail:
2424
+ for line in textwrap.wrap(item.detail, 68):
2425
+ print(f" {DIM}{line}{R}")
2426
+ if item.question and interactive:
2427
+ confirmed = _ask_pilot(item.question)
2428
+ item.user_confirmed = confirmed
2429
+ if confirmed:
2430
+ print(f" {G}→ Acknowledged by pilot{R}")
2431
+ else:
2432
+ print(f" {RED}→ Pilot says NO — review this before flying{R}")
2433
+ n_fail += 1
2434
+ continue
2435
+ elif item.recommendation:
2436
+ print(f" {Y}→ {item.recommendation}{R}")
2437
+ if item.cli_fix:
2438
+ print(f" {C}CLI: {item.cli_fix}{R}")
2439
+ n_warn += 1
2440
+
2441
+ elif item.status == SanityItem.ASK:
2442
+ print(f" {C}? CONFIRM:{R} {item.message}")
2443
+ if item.detail:
2444
+ for line in textwrap.wrap(item.detail, 68):
2445
+ print(f" {DIM}{line}{R}")
2446
+ if item.question and interactive:
2447
+ confirmed = _ask_pilot(item.question)
2448
+ item.user_confirmed = confirmed
2449
+ if confirmed:
2450
+ print(f" {G}→ Confirmed{R}")
2451
+ n_pass += 1
2452
+ else:
2453
+ print(f" {RED}→ NOT confirmed — fix this before flying{R}")
2454
+ if item.recommendation:
2455
+ print(f" {Y}→ {item.recommendation}{R}")
2456
+ if item.cli_fix:
2457
+ print(f" {C}CLI: {item.cli_fix}{R}")
2458
+ n_fail += 1
2459
+ else:
2460
+ # Non-interactive: treat ASK as warning
2461
+ if item.recommendation:
2462
+ print(f" {Y}→ {item.recommendation}{R}")
2463
+ n_ask_unconfirmed += 1
2464
+
2465
+ print()
2466
+
2467
+ # ── VERDICT ──────────────────────────────────────────────────────────
2468
+ print(f" {B}{'=' * 55}{R}")
2469
+ print(f" {B} PRE-FLIGHT VERDICT{R}")
2470
+ print(f" {B}{'=' * 55}{R}")
2471
+
2472
+ if n_fail > 0:
2473
+ print(f" {RED}✗{R} {n_fail} CRITICAL issue{'s' if n_fail > 1 else ''} — must fix before flying")
2474
+ if n_warn > 0:
2475
+ print(f" {Y}⚠{R} {n_warn} WARNING{'s' if n_warn > 1 else ''} — review recommended")
2476
+ if n_ask_unconfirmed > 0:
2477
+ print(f" {C}?{R} {n_ask_unconfirmed} item{'s' if n_ask_unconfirmed > 1 else ''} need{'s' if n_ask_unconfirmed == 1 else ''} pilot confirmation (use --check without --no-interactive)")
2478
+ if n_pass > 0:
2479
+ print(f" {G}✓{R} {n_pass} check{'s' if n_pass > 1 else ''} passed")
2480
+
2481
+ print()
2482
+ if n_fail > 0:
2483
+ print(f" {RED}{B}██ NO-GO ██{R}")
2484
+ print(f" {RED}Fix critical issues and run --check again.{R}")
2485
+ elif n_warn > 0 or n_ask_unconfirmed > 0:
2486
+ print(f" {Y}{B}██ CONDITIONAL ██{R}")
2487
+ print(f" {Y}Review warnings before flying.{R}")
2488
+ else:
2489
+ print(f" {G}{B}██ GO ██{R}")
2490
+ print(f" {G}All checks passed. Fly safe!{R}")
2491
+ print()
2492
+
2493
+ return n_fail
2494
+
2495
+
2496
+ def _pull_diff_from_device(device_path, no_color=False):
2497
+ """Connect to FC via MSP and pull diff all. Returns (text, info) or exits."""
2498
+ R, B, C, G, Y, RED, DIM = _colors()
2499
+
2500
+ try:
2501
+ from inav_toolkit.msp import auto_detect_fc, INAVDevice
2502
+ except ImportError:
2503
+ from msp import auto_detect_fc, INAVDevice
2504
+
2505
+ print(f" {B}Connecting to FC...{R}", end=" ", flush=True)
2506
+
2507
+ if device_path == "auto":
2508
+ dev, info = auto_detect_fc()
2509
+ if not dev:
2510
+ print(f"\n {RED}ERROR: No INAV flight controller found.{R}")
2511
+ sys.exit(1)
2512
+ else:
2513
+ dev = INAVDevice(device_path)
2514
+ dev.open()
2515
+ info = dev.get_info()
2516
+
2517
+ fc_name = info.get("craft_name", "Unknown")
2518
+ fw_ver = info.get("fw_version", "?")
2519
+ board = info.get("board_id", "?")
2520
+ print(f"{C}{board}{R} — {fc_name} — INAV {fw_ver}")
2521
+
2522
+ print(f" Pulling configuration...", end=" ", flush=True)
2523
+ try:
2524
+ diff_text = dev.get_diff_all(timeout=15.0)
2525
+ print(f"done ({len(diff_text.splitlines())} lines)")
2526
+ except Exception as e:
2527
+ print(f"\n {RED}ERROR: Failed to pull diff: {e}{R}")
2528
+ dev.close()
2529
+ sys.exit(1)
2530
+
2531
+ dev.close()
2532
+ return diff_text
2533
+
2534
+
2535
+ # ─── Reporting ───────────────────────────────────────────────────────────────
2536
+
2537
+ def print_report(parsed, findings, frame_inches=None):
2538
+ R, B, C, G, Y, RED, DIM = _colors()
2539
+
2540
+ sev_color = {CRITICAL: RED, WARNING: Y, INFO: C, OK: G}
2541
+ sev_icon = {CRITICAL: "✗", WARNING: "⚠", INFO: "ℹ", OK: "✓"}
2542
+
2543
+ print(f"\n{B}{C}{'═'*70}{R}")
2544
+ print(f"{B}{C} INAV Parameter Analyzer v{VERSION}{R}")
2545
+ print(f"{B}{C}{'═'*70}{R}")
2546
+
2547
+ # Header info
2548
+ if parsed["version"]:
2549
+ print(f" {DIM}Firmware: INAV {parsed['version']} | Board: {parsed['board']}{R}")
2550
+ name = get_setting(parsed, "name", "")
2551
+ if name:
2552
+ print(f" {DIM}Craft: {name}{R}")
2553
+ if frame_inches:
2554
+ print(f" {DIM}Profile: {frame_inches}-inch{R}")
2555
+ pnum = parsed["active_control_profile"]
2556
+ print(f" {DIM}Active control profile: {pnum}{R}")
2557
+
2558
+ # Summary counts
2559
+ counts = {CRITICAL: 0, WARNING: 0, INFO: 0, OK: 0}
2560
+ for f in findings:
2561
+ counts[f.severity] = counts.get(f.severity, 0) + 1
2562
+
2563
+ print(f"\n {B}SUMMARY:{R}")
2564
+ if counts[CRITICAL]:
2565
+ print(f" {RED}{B}{counts[CRITICAL]} CRITICAL{R} - fix before flying")
2566
+ if counts[WARNING]:
2567
+ print(f" {Y}{B}{counts[WARNING]} WARNING{R} - should address")
2568
+ if counts[INFO]:
2569
+ print(f" {C}{counts[INFO]} suggestions{R}")
2570
+ if counts[OK]:
2571
+ print(f" {G}{counts[OK]} checks passed{R}")
2572
+
2573
+ # Group findings by category
2574
+ categories = {}
2575
+ for f in findings:
2576
+ if f.category not in categories:
2577
+ categories[f.category] = []
2578
+ categories[f.category].append(f)
2579
+
2580
+ # Print non-OK findings first, grouped by category
2581
+ has_issues = any(f.severity != OK for f in findings)
2582
+ if has_issues:
2583
+ print(f"\n{B}{C}{'─'*70}{R}")
2584
+ print(f" {B}FINDINGS:{R}")
2585
+ print(f"{B}{C}{'─'*70}{R}")
2586
+
2587
+ for cat, cat_findings in categories.items():
2588
+ issues = [f for f in cat_findings if f.severity != OK]
2589
+ if not issues:
2590
+ continue
2591
+ print(f"\n {B}{cat}{R}")
2592
+ for f in issues:
2593
+ sc = sev_color[f.severity]
2594
+ icon = sev_icon[f.severity]
2595
+ print(f" {sc}{B}{icon}{R} {B}{f.title}{R}")
2596
+ for line in textwrap.wrap(f.detail, width=62):
2597
+ print(f" {DIM}{line}{R}")
2598
+ if f.current:
2599
+ print(f" {DIM}Current: {f.current}{R}")
2600
+ if f.recommended:
2601
+ print(f" {DIM}Recommended: {f.recommended}{R}")
2602
+
2603
+ # CLI fixes
2604
+ fixes = [f for f in findings if f.cli_fix and f.severity in (CRITICAL, WARNING)]
2605
+ if fixes:
2606
+ print(f"\n{B}{C}{'─'*70}{R}")
2607
+ print(f" {B}SUGGESTED CLI FIXES:{R}")
2608
+ print(f"{B}{C}{'─'*70}{R}")
2609
+ print()
2610
+ for f in fixes:
2611
+ print(f" {DIM}# {f.title}{R}")
2612
+ for cmd in f.cli_fix.split("\n"):
2613
+ print(f" {G}{cmd}{R}")
2614
+ print()
2615
+ print(f" {G}save{R}")
2616
+
2617
+ # OK items (compact)
2618
+ ok_items = [f for f in findings if f.severity == OK]
2619
+ if ok_items:
2620
+ print(f"\n{B}{C}{'─'*70}{R}")
2621
+ print(f" {B}PASSED:{R}")
2622
+ for f in ok_items:
2623
+ print(f" {G}✓{R} {DIM}{f.title}{R}")
2624
+
2625
+ print(f"\n{B}{C}{'═'*70}{R}\n")
2626
+
2627
+
2628
+ # ─── Main ────────────────────────────────────────────────────────────────────
2629
+
2630
+ def main():
2631
+ parser = argparse.ArgumentParser(
2632
+ description=f"INAV Parameter Analyzer v{VERSION} - Check diff all for issues",
2633
+ formatter_class=argparse.RawDescriptionHelpFormatter,
2634
+ epilog=textwrap.dedent("""\
2635
+ Modes:
2636
+ Analysis (default): inav-params diff_all.txt --frame 10
2637
+ Setup: inav-params --setup 10 --voltage 6S
2638
+ Sanity check: inav-params --check diff_all.txt
2639
+ Sanity check (USB): inav-params --check --device auto
2640
+
2641
+ Supported frame sizes: 5, 7, 10, 12, 15 inches
2642
+ Supported voltages: 4S, 6S, 8S, 12S
2643
+ """))
2644
+ parser.add_argument("--version", action="version", version=f"inav-params {VERSION}")
2645
+ parser.add_argument("difffile", nargs="?", default=None,
2646
+ help="INAV `diff all` output (.txt file or stdin)")
2647
+ parser.add_argument("--frame", type=int, metavar="INCHES",
2648
+ help="Frame size in inches (affects filter/PID recommendations)")
2649
+ parser.add_argument("--blackbox", metavar="STATE_JSON",
2650
+ help="Blackbox analyzer state.json for cross-reference")
2651
+ parser.add_argument("--json", action="store_true",
2652
+ help="Output findings as JSON")
2653
+ parser.add_argument("--setup", type=int, metavar="INCHES",
2654
+ help="Generate starting configuration for a new quad (7/10/12/15)")
2655
+ parser.add_argument("--voltage", type=str, metavar="CELLS", default="4S",
2656
+ help="Battery voltage for --setup (4S/6S/8S/12S, default: 4S)")
2657
+ parser.add_argument("--check", action="store_true",
2658
+ help="Pre-flight sanity check mode - catches dangerous misconfigurations")
2659
+ parser.add_argument("--device", type=str, metavar="PORT", default=None,
2660
+ help="FC serial port for --check mode (use 'auto' to scan)")
2661
+ parser.add_argument("--no-interactive", action="store_true",
2662
+ help="Skip interactive questions in --check mode")
2663
+ parser.add_argument("--no-color", action="store_true",
2664
+ help="Disable colored terminal output.")
2665
+ parser.add_argument("--lang", metavar="LANG",
2666
+ help="Language for output (en, pt_BR, es). "
2667
+ "Auto-detects from INAV_LANG env var or system locale.")
2668
+ args = parser.parse_args()
2669
+
2670
+ # Initialize localization
2671
+ try:
2672
+ from inav_toolkit.i18n import set_locale, detect_locale
2673
+ except ImportError:
2674
+ try:
2675
+ from i18n import set_locale, detect_locale
2676
+ except ImportError:
2677
+ set_locale = detect_locale = None
2678
+ if set_locale and detect_locale:
2679
+ lang = getattr(args, 'lang', None) or detect_locale()
2680
+ set_locale(lang)
2681
+
2682
+ if args.no_color:
2683
+ _disable_colors()
2684
+
2685
+ # ─── Sanity check mode ─────────────────────────────────────────────
2686
+ if args.check:
2687
+ if args.device:
2688
+ text = _pull_diff_from_device(args.device, args.no_color)
2689
+ elif args.difffile:
2690
+ if args.difffile == "-":
2691
+ text = sys.stdin.read()
2692
+ else:
2693
+ if not os.path.isfile(args.difffile):
2694
+ print(f"ERROR: File not found: {args.difffile}")
2695
+ sys.exit(1)
2696
+ with open(args.difffile, "r", errors="replace") as f:
2697
+ text = f.read()
2698
+ else:
2699
+ parser.error("--check requires a diff file or --device")
2700
+
2701
+ parsed = parse_diff_all(text)
2702
+ interactive = not args.no_interactive
2703
+ items = run_sanity_check(parsed, frame_inches=args.frame,
2704
+ interactive=interactive)
2705
+
2706
+ if args.json:
2707
+ output = [{
2708
+ "status": i.status,
2709
+ "category": i.category,
2710
+ "message": i.message,
2711
+ "detail": i.detail,
2712
+ "recommendation": i.recommendation,
2713
+ "cli_fix": i.cli_fix,
2714
+ "user_confirmed": i.user_confirmed,
2715
+ } for i in items]
2716
+ print(json.dumps(output, indent=2))
2717
+ else:
2718
+ n_fail = print_sanity_report(items, parsed, interactive=interactive)
2719
+ sys.exit(1 if n_fail > 0 else 0)
2720
+ return
2721
+ args = parser.parse_args()
2722
+
2723
+ if args.no_color:
2724
+ _disable_colors()
2725
+
2726
+ # ─── Setup mode ───────────────────────────────────────────────────────
2727
+ if args.setup:
2728
+ if not args.json:
2729
+ print(f"\n ▲ INAV Parameter Analyzer v{VERSION}")
2730
+ print(f" Mode: Starting configuration")
2731
+
2732
+ voltage = args.voltage.upper()
2733
+ if not voltage.endswith("S"):
2734
+ voltage = voltage + "S"
2735
+
2736
+ config = generate_setup_config(args.setup, voltage=voltage)
2737
+
2738
+ if args.json:
2739
+ print_setup_json(config)
2740
+ else:
2741
+ print_setup_report(config)
2742
+
2743
+ # If a diff file was also provided, show what would change
2744
+ if args.difffile and os.path.isfile(args.difffile):
2745
+ with open(args.difffile, "r", errors="replace") as f:
2746
+ text = f.read()
2747
+ parsed = parse_diff_all(text)
2748
+ profile = get_active_control(parsed)
2749
+
2750
+ R, B, C, G, Y, _RED, DIM = _colors()
2751
+ print(f" {B}COMPARISON WITH CURRENT CONFIG ({args.difffile}):{R}")
2752
+ print(f" {'Current':>8} {'Suggested':>10} {'Change':>8}")
2753
+
2754
+ all_new = {}
2755
+ all_new.update(config["pids"])
2756
+ all_new.update(config["filters"])
2757
+ all_new.update(config["other"])
2758
+
2759
+ for k, new_val in sorted(all_new.items()):
2760
+ cur = profile.get(k, get_setting(parsed, k, None))
2761
+ if cur is not None and cur != new_val:
2762
+ if isinstance(cur, (int, float)) and isinstance(new_val, (int, float)):
2763
+ diff = new_val - cur
2764
+ sign = "+" if diff > 0 else ""
2765
+ color = G if abs(diff) < abs(cur) * 0.05 else Y
2766
+ print(f" {k:26} {cur:>8} {new_val:>10} {color}{sign}{diff}{R}")
2767
+ else:
2768
+ print(f" {k:26} {str(cur):>8} {str(new_val):>10}")
2769
+
2770
+ print()
2771
+ return
2772
+
2773
+ # ─── Analysis mode ────────────────────────────────────────────────────
2774
+ if not args.difffile:
2775
+ parser.error("diff file is required (or use --setup INCHES)")
2776
+
2777
+ # Load diff all
2778
+ if args.difffile == "-":
2779
+ text = sys.stdin.read()
2780
+ else:
2781
+ if not os.path.isfile(args.difffile):
2782
+ print(f"ERROR: File not found: {args.difffile}")
2783
+ sys.exit(1)
2784
+ with open(args.difffile, "r", errors="replace") as f:
2785
+ text = f.read()
2786
+
2787
+ if not args.json:
2788
+ print(f"\n ▲ INAV Parameter Analyzer v{VERSION}")
2789
+ print(f" Loading: {args.difffile}")
2790
+
2791
+ parsed = parse_diff_all(text)
2792
+
2793
+ if not args.json:
2794
+ if parsed["version"]:
2795
+ print(f" Firmware: INAV {parsed['version']} on {parsed['board']}")
2796
+ name = get_setting(parsed, "name", "")
2797
+ if name:
2798
+ print(f" Craft: {name}")
2799
+
2800
+ # Load blackbox state if provided
2801
+ bb_state = None
2802
+ if args.blackbox:
2803
+ try:
2804
+ with open(args.blackbox) as f:
2805
+ bb_state = json.load(f)
2806
+ if not args.json:
2807
+ print(f" Blackbox state: {args.blackbox}")
2808
+ except Exception as e:
2809
+ if not args.json:
2810
+ print(f" ⚠ Could not load blackbox state: {e}")
2811
+
2812
+ # Run checks
2813
+ findings = run_all_checks(parsed, frame_inches=args.frame, blackbox_state=bb_state)
2814
+
2815
+ if args.json:
2816
+ output = [{
2817
+ "severity": f.severity,
2818
+ "category": f.category,
2819
+ "title": f.title,
2820
+ "detail": f.detail,
2821
+ "setting": f.setting,
2822
+ "current": f.current,
2823
+ "recommended": f.recommended,
2824
+ "cli_fix": f.cli_fix,
2825
+ } for f in findings]
2826
+ print(json.dumps(output, indent=2))
2827
+ else:
2828
+ print_report(parsed, findings, frame_inches=args.frame)
2829
+
2830
+
2831
+ if __name__ == "__main__":
2832
+ main()