diff --git a/OVERVIEW.md b/OVERVIEW.md index bc933e7..d34bb20 100644 --- a/OVERVIEW.md +++ b/OVERVIEW.md @@ -9,6 +9,7 @@ - [Implementation Details](#implementation-details) - [Filter Response Curves](#filter-response-curves) - [Bode Plot Analysis (Optional)](#bode-plot-analysis-optional) + - [Optimal P Estimation (Optional, Experimental)](#optimal-p-estimation-optional-experimental) - [Step-Response Comparison with Other Analysis Tools](#step-response-comparison-with-other-analysis-tools) - [Compared to PIDtoolbox/Matlab (PTstepcalc.m)](#compared-to-pidtoolboxmatlab-ptstepcalcm) - [Compared to PlasmaTree/Python (PID-Analyzer.py)](#compared-to-plasmatreepython-pid-analyzerpy) @@ -37,7 +38,7 @@ All analysis parameters, thresholds, plot dimensions, and algorithmic constants * This is the core of the step response analysis. It implements **non-parametric system identification** using Wiener deconvolution rather than traditional first-order or second-order curve fitting. This approach directly extracts the system's actual step response without assuming a specific mathematical model, allowing it to capture complex, higher-order dynamics and non-linearities. * For each axis (Roll, Pitch, Yaw): * It takes the prepared time, setpoint, and (optionally smoothed via `INITIAL_GYRO_SMOOTHING_WINDOW`) gyro data arrays and the sample rate. - * **Windowing:** The input signals (setpoint and gyro) are segmented into overlapping windows (`winstacker_contiguous`) of `FRAME_LENGTH_S` duration. A Tukey window (`tukeywin` with `TUKEY_ALPHA`) is applied to each segment to reduce spectral leakage. + * **Windowing:** The input signals (setpoint and gyro) are segmented into overlapping windows (`winstacker_contiguous`) of `FRAME_LENGTH_S` duration — intentionally longer than the displayed `RESPONSE_LENGTH_S` to improve frequency-domain resolution for the Wiener deconvolution; only the first `RESPONSE_LENGTH_S` of each estimated step response is retained for display and analysis. A Tukey window (`tukeywin` with `TUKEY_ALPHA`) is applied to each segment to reduce spectral leakage. * **Movement Threshold:** Windows are discarded if the maximum absolute setpoint value within them is below `MOVEMENT_THRESHOLD_DEG_S`. * **Deconvolution:** For each valid window, Wiener deconvolution (`wiener_deconvolution_window`) is performed between the windowed setpoint (input) and gyro (output) signals in the frequency domain. This estimates the impulse response of the system. A regularization term (`0.0001`) helps stabilize the deconvolution. * **Impulse to Step Response:** The resulting impulse response is converted to a step response by cumulative summation (`cumulative_sum`). This step response is then truncated to `RESPONSE_LENGTH_S`. @@ -189,6 +190,71 @@ The system provides intelligent P:D tuning recommendations based on step-respons - Shows recommendations only when the step response needs improvement (skips optimal peak 0.95–1.04) - **Note:** Peak value measures the first maximum after crossing the setpoint; the initial transient dip is normal system behavior +#### Optimal P Estimation (Optional, Experimental) + +Physics-derived P gain optimization using a Torque-Inertia Profiler that measures aircraft-specific dynamics directly from flight log throttle-punch events. No prop-size input is required — the aircraft's torque-to-inertia ratio is derived from the logs. + +- **Activation:** Disabled by default; enable with `--estimate-optimal-p` flag. +- **Requires:** A `.headers.csv` metadata file alongside each input CSV (produced by `blackbox_decode`). Without it, P gain values are unavailable and optimal P estimation is skipped with a skip-reason shown in console and PNG. All other analyses (step response, spectrums, P:D recommendations) remain unaffected. +- **⚠️ Status:** Experimental. `TORQUE_PROFILER_ACHIEVABILITY_FACTOR` bridges the gap between the theoretical physics formula and real-world flight performance (ESC lag, prop-wash, motor startup). It is empirically calibrated and may need adjustment for aircraft significantly different from a mid-size freestyle build. + +##### Torque-Inertia Profiler (`src/data_analysis/torque_inertia_profiler.rs`) + +- **Phase 1 — Aircraft Profiling (per group, before per-file processing):** + - All logs sharing an aircraft key are processed together via `profile_aircraft_group()`. + - `extract_punch_ratios()` detects throttle-punch events: `setpoint[3]` increases ≥ `THROTTLE_PUNCH_MIN_DELTA` within `THROTTLE_PUNCH_WINDOW_MS`. + - For each punch, peak angular acceleration `|Δgyro/Δt|` in the response window (after `TORQUE_PROFILER_SETTLE_MS` of ESC/motor settle time, converted to samples at runtime from the actual log sample rate) is divided by the normalised throttle command delta → `torque_inertia_ratio`. + - Ratios are aggregated into `AircraftProfile` (median + half-IQR spread per axis, Roll and Pitch only). Yaw ratios are collected but not used in optimal-P analysis because Yaw dynamics differ from Roll/Pitch and the current formula is not calibrated for Yaw. + - Requires ≥ `TORQUE_PROFILER_MIN_EVENTS` punch events. If insufficient, a skip message appears in both console and PNG overlay. + +- **Phase 2 — Per-File Optimal-P Analysis:** + - Physics formula: `Td_ms = TORQUE_PROFILER_TD_CALC_K / sqrt((P / TORQUE_PROFILER_P_SCALE) × torque_inertia_ratio) × TORQUE_PROFILER_ACHIEVABILITY_FACTOR` + - Per-file Td samples are collected from all valid step response windows and compared against the physics-derived target. + - HF noise energy from D-term spectral analysis informs whether noise limits further P increase. + - `OptimalPAnalysis` classifies the result as `Increase`, `Optimal`, `Decrease`, or `Investigate`. + +- **Aircraft Grouping (`extract_aircraft_key()`):** + - Strips `_YYYYMMDD_HHMMSS` timestamp from filename stem so logs from the same aircraft across multiple sessions share one key. + - When the prefix ends with `BLACKBOX_LOG` (generic Betaflight/EmuFlight naming), the craft name following the timestamp is appended to prevent distinct aircraft from collapsing into one group (e.g., `BTFL_BLACKBOX_LOG_YYYYMMDD_HHMMSS_CRAFTNAME.NN.csv` → key `BTFL_BLACKBOX_LOG_CRAFTNAME`). + +- **Key Constants (`src/constants.rs`):** + - `THROTTLE_PUNCH_MIN_DELTA` — minimum throttle step (0–1000 units) to qualify as a punch + - `THROTTLE_PUNCH_WINDOW_MS` — detection window for the throttle rise + - `THROTTLE_RESPONSE_WINDOW_MS` — gyro response measurement window + - `TORQUE_PROFILER_SETTLE_MS` — settle time (ms) skipped at response start (ESC/motor lag allowance); converted to samples at runtime so it is correct at all loop rates + - `TORQUE_PROFILER_MIN_EVENTS` — minimum punches required for a reliable profile + - `TORQUE_PROFILER_P_SCALE` — converts raw firmware P gain to physical units + - `TORQUE_PROFILER_TD_CALC_K` — Td numerator constant (π × 500) + - `TORQUE_PROFILER_ACHIEVABILITY_FACTOR` — empirical calibration coefficient + +- **Recommendation Types:** + - **P Increase:** Td slower than target with acceptable noise → P is conservative + - **Optimal:** Td within target range or at physical limits → P is well-matched + - **P Decrease:** Td faster than target with high noise → P is too high (rare) + - **Investigate:** Measurements suggest mechanical issues or abnormal dynamics + +- **Output:** Console report and PNG legend overlay showing: Td mean with `windows=` count (valid step-response windows contributing to the Td mean), target, deviation %, noise level, consistency % (orange warning when CV exceeds `TD_COEFFICIENT_OF_VARIATION_MAX`), `[LOW AUTHORITY]` warning when max setpoint is below `LOW_AUTHORITY_SETPOINT_THRESHOLD_DEG_S`, and P recommendation with calculated D adjustment. When profiling is skipped (insufficient punch events), a skip reason appears in both outputs. See **Consistency and Reliability Interpretation** below for signal details. + +- **Consistency and Reliability Interpretation (CV):** + - **CV (Coefficient of Variation)** = standard deviation / mean of individual Td measurements across all valid step-response windows. It quantifies how scattered the measurements are relative to their average. + - **Low CV:** Td measurements are tightly clustered — the log contains clean, repeatable dynamics and recommendations are trustworthy. + - **High CV (exceeds `TD_COEFFICIENT_OF_VARIATION_MAX`):** Td measurements vary widely across windows — a consistency warning is shown in both console and PNG. Recommendations should be treated with caution. + - **CV = None:** Fewer than `TD_SAMPLES_MIN_FOR_STDDEV` valid Td samples were available; standard deviation cannot be computed. The mean is still reported but no consistency rating is given. + - **Low-authority flight warning:** When the maximum setpoint across all valid windows is below `LOW_AUTHORITY_SETPOINT_THRESHOLD_DEG_S`, a `[LOW AUTHORITY]` warning is shown in both console and PNG. Hover tests and slow-cruise logs never produce sharp inputs — step-response analysis and Td measurements from such logs are noise-dominated rather than dynamics-dominated, making all recommendations unreliable regardless of CV. + - **Why hover logs produce high CV:** Small setpoint inputs → deconvolution is noise-sensitive → each window captures a different noise realisation. The averaged response may appear plausible (noise averages out) while individual window variance remains high. CV exposes this where the mean alone cannot. + - **Over-P limitation:** **When P is already too high and the aircraft oscillates, the profiler may report "Optimal" rather than "Decrease P."** An oscillatory step response produces a short, aggressive measured Td — which, fed into the physics formula, yields a P_optimal close to the current (excessive) P. The profiler cannot reliably distinguish a well-tuned fast response from an over-tuned oscillating one using Td alone. The indirect signal is CV: severe oscillation typically scatters Td samples widely and triggers the consistency warning. **If your gains feel high or the craft exhibits oscillation, start from a lower P before relying on these recommendations.** Optimal P estimation is most accurate when the craft is in a reasonable tuning range — it is a validator and refinement tool, not a recovery tool for badly mis-tuned aircraft. Only experienced pilots are likely to recognise this situation by feel. + - **High CV without `[LOW AUTHORITY]`:** If the consistency warning fires but `[LOW AUTHORITY]` is not shown, the scatter is not caused by low-energy hover inputs. Remaining causes include propwash, inconsistent maneuvers, and oscillation from over-P. **If gains feel high, treat this combination as a prompt to verify the craft is not oscillating before acting on any recommendation.** + - **Summary of dependability signals in output:** + - `windows=` on the Td line — number of valid step-response windows contributing to the Td mean; more windows = more statistical weight + - Consistency % and CV — how repeatable individual measurements are + - `[LOW AUTHORITY]` — max setpoint too small for reliable step-response characterisation + - Noise level (`LOW` / `MODERATE` / `HIGH`) — HF D-term energy; high noise limits safe P increase + +- **Relationship to P:D Recommendations:** + - P:D ratio recommendations: analyze peak overshoot → adjust D relative to P + - Optimal P estimation: analyze response timing → adjust P magnitude + - Both features are complementary; both appear in console output and PNG legend simultaneously + ### Step-Response Comparison with Other Analysis Tools This implementation provides detailed and configurable analysis of flight controller performance. The modular design and centralized configuration system make it adaptable for various analysis requirements. diff --git a/README.md b/README.md index bad5e55..3264c0b 100644 --- a/README.md +++ b/README.md @@ -48,6 +48,7 @@ Usage: ./BlackBox_CSV_Render [ ...] [OPTIONS] --butterworth: Show Butterworth PT1 cutoffs on gyro/D-term spectrum plots. --dps : Deg/s threshold for detailed step response plots (positive number). + --estimate-optimal-p: Enable optimal P estimation from throttle-punch dynamics. Requires .headers.csv; skips P estimation if absent. === GENERAL === @@ -76,6 +77,9 @@ Arguments can be in any order. Wildcards (e.g., *.csv) are shell-expanded and wo ```shell ./target/release/BlackBox_CSV_Render path/to/ --step --setpoint --motor --output-dir ./all-selective ``` +```shell +./target/release/BlackBox_CSV_Render path/to/BTFL_Log.csv --step --estimate-optimal-p +``` ### Output @@ -100,6 +104,7 @@ Arguments can be in any order. Wildcards (e.g., *.csv) are shell-expanded and wo - Warning indicators for severe overshoot or unreasonable ratios - Gyro filtering delay estimates (filtered vs. unfiltered, with confidence) - Filter configuration parsing and spectrum peak detection summaries +- Optimal P estimation (`--estimate-optimal-p`): Td timing, target deviation, noise level, consistency, P/D recommendations and skip-reason warnings - Use `--debug` flag for additional metadata: header information, flight data key mapping, sample header values, and debug mode identification #### Code and Output Overview diff --git a/src/axis_names.rs b/src/axis_names.rs index 811b8e5..53b6fa5 100644 --- a/src/axis_names.rs +++ b/src/axis_names.rs @@ -13,25 +13,29 @@ /// Panics if index is greater than 2 #[allow(dead_code)] pub fn axis_name(index: usize) -> &'static str { - match index { - 0 => "Roll", - 1 => "Pitch", - 2 => "Yaw", - _ => panic!( + AXIS_NAMES.get(index).copied().unwrap_or_else(|| { + panic!( "Invalid axis index: {}. Expected 0 (Roll), 1 (Pitch), or 2 (Yaw)", index - ), - } + ) + }) } /// Number of axes (Roll, Pitch, Yaw) pub const AXIS_COUNT: usize = 3; +/// Number of primary control axes (Roll, Pitch only - excludes Yaw) +pub const ROLL_PITCH_AXIS_COUNT: usize = 2; + /// Get all axis names as a static array pub const AXIS_NAMES: [&str; AXIS_COUNT] = ["Roll", "Pitch", "Yaw"]; -// Compile-time check to prevent drift between AXIS_COUNT and AXIS_NAMES.len() +// Compile-time assertions to prevent invariant drift const _: [(); AXIS_COUNT] = [(); AXIS_NAMES.len()]; +const _: () = assert!( + ROLL_PITCH_AXIS_COUNT > 0 && ROLL_PITCH_AXIS_COUNT < AXIS_COUNT, + "ROLL_PITCH_AXIS_COUNT must be > 0 and < AXIS_COUNT" +); #[cfg(test)] mod tests { diff --git a/src/constants.rs b/src/constants.rs index e4717a1..e0ad3bd 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -49,6 +49,10 @@ pub const DEFAULT_SETPOINT_THRESHOLD: f64 = 500.0; // Default setpoint threshold // Constants for filtering data based on movement and flight phase. pub const MOVEMENT_THRESHOLD_DEG_S: f64 = 20.0; // Minimum setpoint/gyro magnitude (from PTB/PlasmaTree) +/// Max setpoint (deg/s) below which a flight is considered low-authority. +/// Hover tests and slow-cruise logs never produce sharp inputs, so step-response +/// quality and P:D/optimal-P recommendations from such logs are unreliable. +pub const LOW_AUTHORITY_SETPOINT_THRESHOLD_DEG_S: f32 = 100.0; pub const EXCLUDE_START_S: f64 = 3.0; // Exclude seconds from the start of the log pub const EXCLUDE_END_S: f64 = 3.0; // Exclude seconds from the end of the log @@ -189,6 +193,14 @@ pub const COLOR_STEP_RESPONSE_LOW_SP: &RGBColor = &LIGHTBLUE; pub const COLOR_STEP_RESPONSE_HIGH_SP: &RGBColor = &ORANGE; pub const COLOR_STEP_RESPONSE_COMBINED: &RGBColor = &RED; +// Optimal P Estimation Legend Colors +pub const COLOR_OPTIMAL_P_DIVIDER: RGBColor = RGBColor(40, 40, 40); +pub const COLOR_OPTIMAL_P_HEADER: RGBColor = RGBColor(0, 100, 200); +pub const COLOR_OPTIMAL_P_TEXT: RGBColor = RGBColor(80, 80, 80); +pub const COLOR_OPTIMAL_P_WARNING: RGBColor = RGBColor(200, 100, 0); +pub const COLOR_OPTIMAL_P_RECOMMENDATION: RGBColor = RGBColor(0, 150, 0); +pub const COLOR_OPTIMAL_P_SKIP: RGBColor = RGBColor(120, 60, 60); + // Stroke widths for lines pub const LINE_WIDTH_PLOT: u32 = 1; // Width for plot lines pub const LINE_WIDTH_LEGEND: u32 = 2; // Width for legend lines @@ -259,4 +271,100 @@ pub const PSD_EPSILON: f64 = 1e-12; // Guard against division by zero for PSD va pub const MAGNITUDE_PLOT_MARGIN_DB: f64 = 10.0; // Padding above/below magnitude data for plot range pub const PHASE_PLOT_MARGIN_DEG: f64 = 30.0; // Padding above/below phase data for plot range -// src/constants.rs +// High-frequency noise analysis for P headroom estimation +// D-term energy above this frequency threshold indicates noise constraints +pub const DTERM_HF_CUTOFF_HZ: f64 = 200.0; // Frequency above which high-frequency noise is measured +pub const DTERM_HF_ENERGY_THRESHOLD: f64 = 0.15; // 15% of total D-term energy (high noise level) +pub const DTERM_HF_ENERGY_MODERATE: f64 = 0.10; // 10% of total D-term energy (moderate noise level) + +// Response consistency quality control +// Ensures Td measurements are reliable across multiple step responses +pub const TD_CONSISTENCY_MIN_THRESHOLD: f64 = 0.70; // 70% of responses within tolerance (was 85%) +pub const TD_COEFFICIENT_OF_VARIATION_MAX: f64 = 0.40; // 40% CV (std/mean) is acceptable for noisy flight logs (was 20%) + +// P headroom estimation multipliers +// Conservative approach for users who want safe incremental improvements +pub const P_HEADROOM_CONSERVATIVE_MULTIPLIER: f64 = 1.05; // +5% from current P + // Moderate approach for experienced pilots +pub const P_HEADROOM_MODERATE_MULTIPLIER: f64 = 1.10; // +10% from current P + // Aggressive approach for optimization (use with caution) +#[allow(dead_code)] +pub const P_HEADROOM_AGGRESSIVE_MULTIPLIER: f64 = 1.15; // +15% from current P (reserved for future use) + +// P reduction multipliers (when Td is too fast or noise is too high) +pub const P_REDUCTION_MODERATE_MULTIPLIER: f64 = 0.95; // -5% from current P +#[allow(dead_code)] +pub const P_REDUCTION_AGGRESSIVE_MULTIPLIER: f64 = 0.90; // -10% from current P + +// Td statistics computation constants +pub const MIN_TD_MS: f64 = 0.1; // Minimum valid Td (time to 50%) in milliseconds (domain-appropriate threshold) +pub const TD_MEAN_EPSILON: f64 = 1e-12; // Threshold for near-zero mean values (avoid division by zero) +pub const TD_SAMPLES_MIN_FOR_STDDEV: usize = 2; // Minimum samples needed for std dev calculation + +// Td deviation thresholds (percentage deviation from target) +// Deviation thresholds for classifying Td behavior +// Note: The thresholds are intentionally asymmetric — there is no separate +// 'moderately faster' threshold. Faster-than-target deviations are treated +// more strictly because they often indicate potential oscillation or unsafe +// aggressive tuning. Therefore any significant speed-up beyond +// TD_DEVIATION_SIGNIFICANTLY_FASTER_THRESHOLD is flagged immediately. Slower +// deviations are given two thresholds (moderate and significant) to allow +// finer-grained handling when Td is lagging behind the target. +pub const TD_DEVIATION_SIGNIFICANTLY_SLOWER_THRESHOLD: f64 = 30.0; // > 30% slower +pub const TD_DEVIATION_MODERATELY_SLOWER_THRESHOLD: f64 = 15.0; // > 15% slower +pub const TD_DEVIATION_SIGNIFICANTLY_FASTER_THRESHOLD: f64 = -15.0; // < -15% faster + +// Optimal P estimation data collection thresholds +pub const OPTIMAL_P_MIN_DTERM_SAMPLES: usize = 100; // Minimum D-term samples for noise analysis +pub const OPTIMAL_P_SECONDS_TO_MS_MULTIPLIER: f64 = 1000.0; // Convert seconds to milliseconds + +// Torque-Inertia Profiler constants +// Used by torque_inertia_profiler.rs to derive aircraft-specific Td targets from +// throttle-punch events in flight logs, replacing the empirical frame-class table. + +/// Maximum throttle command value in firmware units (setpoint[3] range: 0–1000). +/// Divide raw delta by this to normalise to 0.0–1.0. +pub const THROTTLE_COMMAND_SCALE: f64 = 1000.0; + +/// Minimum time delta (seconds) for angular-acceleration computation in the torque profiler. +/// Guards against division by near-zero or identical consecutive timestamps. +pub const TORQUE_PROFILER_MIN_DT_S: f64 = 1e-9; + +/// Minimum throttle increase (in 0–1000 units) to qualify as a throttle punch. +pub const THROTTLE_PUNCH_MIN_DELTA: f64 = 200.0; + +/// Time window (ms) within which the throttle increase must occur to count as a punch. +pub const THROTTLE_PUNCH_WINDOW_MS: f64 = 50.0; + +/// Window (ms) after punch onset in which to measure peak angular acceleration. +pub const THROTTLE_RESPONSE_WINDOW_MS: f64 = 150.0; + +/// Minimum number of throttle-punch events required for a reliable Td target. +/// Analysis is skipped (with a console warning) when this threshold is not met. +pub const TORQUE_PROFILER_MIN_EVENTS: usize = 5; + +/// Minimum normalised command delta (0–1) for a punch event to be valid. +pub const TORQUE_PROFILER_MIN_CMD_DELTA_NORMALIZED: f64 = 0.10; + +/// Time (ms) to skip at the start of the gyro response window after a throttle punch. +/// Converted to samples at runtime using the actual log sample rate, so it is +/// correct for all loop rates (1 kHz, 3.2 kHz BMI270, 4 kHz, 8 kHz, etc.). +/// 5 ms covers typical DSHOT ESC/motor lag; increase for slow 50 Hz PWM ESCs. +pub const TORQUE_PROFILER_SETTLE_MS: f64 = 5.0; + +/// Numerator constant for Td calculation: K = π × 1000 / 2 +/// Td_ms = K / sqrt((P / P_SCALE) × torque_inertia_ratio) +pub const TORQUE_PROFILER_TD_CALC_K: f64 = 1_570.796_326_794_896_6; + +/// Betaflight/EmuFlight P gain scaling factor. +/// Converts raw firmware P gain (e.g. 45) to an effective physical gain. +/// This constant may require empirical calibration; starting value: 100.0. +pub const TORQUE_PROFILER_P_SCALE: f64 = 100.0; + +/// Real-world achievability factor for physics-derived Td targets. +/// Bridges the gap between theoretical torque capacity and actual flight performance +/// (accounts for ESC lag, motor startup, prop-wash efficiency, etc). +/// Higher values = more relaxed targets. +/// Empirically calibrated on a 5" 6S freestyle build (HELIO H7); may need +/// adjustment for significantly heavier or lighter aircraft classes. +pub const TORQUE_PROFILER_ACHIEVABILITY_FACTOR: f64 = 2.50; diff --git a/src/data_analysis/mod.rs b/src/data_analysis/mod.rs index ee62874..735f5de 100644 --- a/src/data_analysis/mod.rs +++ b/src/data_analysis/mod.rs @@ -6,7 +6,9 @@ pub mod derivative; pub mod fft_utils; pub mod filter_delay; pub mod filter_response; +pub mod optimal_p_estimation; pub mod spectral_analysis; +pub mod torque_inertia_profiler; pub mod transfer_function_estimation; // src/data_analysis/mod.rs diff --git a/src/data_analysis/optimal_p_estimation.rs b/src/data_analysis/optimal_p_estimation.rs new file mode 100644 index 0000000..e4edec9 --- /dev/null +++ b/src/data_analysis/optimal_p_estimation.rs @@ -0,0 +1,605 @@ +// src/data_analysis/optimal_p_estimation.rs +// +// Optimal P Estimation Module +// +// Provides physics-aware P gain recommendations based on: +// - Frame-class-specific Td (time to 50%) targets +// - High-frequency noise level analysis +// - Response consistency metrics +// +// Implements theory from BrianWhite (PIDtoolbox) that optimal response timing +// is aircraft-specific, determined by power-to-rotational-inertia ratio. + +use crate::constants::*; + +/// Error type for optimal P analysis +#[derive(Debug, Clone)] +pub enum AnalysisError { + /// No Td target available for the given frame class + MissingTdTarget { message: String }, +} + +impl std::fmt::Display for AnalysisError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + AnalysisError::MissingTdTarget { message } => { + write!(f, "{}", message) + } + } + } +} + +impl std::error::Error for AnalysisError {} + +/// Safe conversion from scaled f64 to u32 with saturation. +/// +/// Computes (base * multiplier) and returns a saturated `u32` result: +/// - If `multiplier` is NaN, returns 0 (deterministic handling of invalid multiplier). +/// - If the scaled value is >= `u32::MAX`, returns `u32::MAX`. +/// - If the scaled value is <= 0.0, returns 0. +/// - Otherwise returns the truncated `u32` value. +fn safe_scaled_p(base: u32, multiplier: f64) -> u32 { + // Explicitly handle NaN deterministically rather than relying on cast behavior + if multiplier.is_nan() { + return 0; + } + + let scaled = (base as f64) * multiplier; + if scaled.is_infinite() { + if scaled.is_sign_positive() { + return u32::MAX; + } else { + return 0; + } + } + + if scaled >= (u32::MAX as f64) { + u32::MAX + } else if scaled <= 0.0 { + 0 + } else { + scaled as u32 + } +} + +/// Noise level classification +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub enum NoiseLevel { + Low, // < 10% HF energy + Moderate, // 10-15% HF energy + High, // > 15% HF energy + Unknown, // Cannot determine +} + +impl NoiseLevel { + pub fn name(&self) -> &str { + match self { + NoiseLevel::Low => "LOW", + NoiseLevel::Moderate => "MODERATE", + NoiseLevel::High => "HIGH", + NoiseLevel::Unknown => "UNKNOWN", + } + } + + #[allow(dead_code)] + pub fn assessment(&self) -> &str { + match self { + NoiseLevel::Low => "Noise levels are acceptable, P has headroom", + NoiseLevel::Moderate => "Approaching noise limits", + NoiseLevel::High => "At or exceeding recommended noise limits", + NoiseLevel::Unknown => "Cannot assess noise levels (D-term data unavailable)", + } + } +} + +/// Td deviation classification +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum TdDeviation { + SignificantlySlower, // > 30% slower than target + ModeratelySlower, // 15-30% slower + WithinTarget, // ±15% of target + SignificantlyFaster, // < -15% faster than target +} + +impl TdDeviation { + pub fn name(&self) -> &str { + match self { + TdDeviation::SignificantlySlower => "SIGNIFICANTLY SLOWER", + TdDeviation::ModeratelySlower => "MODERATELY SLOWER", + TdDeviation::WithinTarget => "WITHIN TARGET", + TdDeviation::SignificantlyFaster => "FASTER", + } + } +} + +/// P optimization recommendation +#[derive(Debug, Clone, PartialEq)] +pub enum PRecommendation { + Optimal { + reasoning: String, + }, + Increase { + conservative_p: u32, + reasoning: String, + }, + Decrease { + recommended_p: u32, + reasoning: String, + }, + Investigate { + issue: String, + }, +} + +/// Statistics for Td measurements across multiple step responses +#[derive(Debug, Clone)] +pub struct TdStatistics { + pub mean_ms: f64, + pub coefficient_of_variation: Option, + pub num_samples: usize, + pub consistency: f64, // Fraction of samples within ±1 std dev +} + +impl TdStatistics { + /// Calculate statistics from array of Td values (in milliseconds) + pub fn from_samples(td_samples_ms: &[f64]) -> Option { + let td_samples_ms: Vec = td_samples_ms + .iter() + .copied() + .filter(|x| x.is_finite()) + .collect(); + + if td_samples_ms.is_empty() { + return None; + } + + let n = td_samples_ms.len() as f64; + let mean = td_samples_ms.iter().sum::() / n; + + // Use epsilon-based comparison to avoid division by near-zero values + if mean.abs() <= TD_MEAN_EPSILON { + return None; + } + + // Calculate sample variance with Bessel's correction (divide by n-1) + // For small samples, set coefficient_of_variation to None to indicate insufficient data + let coefficient_of_variation = if td_samples_ms.len() < TD_SAMPLES_MIN_FOR_STDDEV { + None + } else { + let sum_sq_dev = td_samples_ms + .iter() + .map(|&x| (x - mean).powi(2)) + .sum::(); + let variance = sum_sq_dev / (n - 1.0); + let std_dev = variance.sqrt(); + Some(std_dev / mean) + }; + + // Calculate consistency: fraction within ±1 std dev + // When coefficient_of_variation is None (too few samples), consistency is perfect (1.0) + // Otherwise, tolerance = cv * mean and calculate fraction within range + let consistency = coefficient_of_variation.map_or(1.0, |cv| { + let tolerance = cv * mean; + let within_range = td_samples_ms + .iter() + .filter(|&&x| (x - mean).abs() <= tolerance) + .count(); + within_range as f64 / n + }); + + Some(TdStatistics { + mean_ms: mean, + coefficient_of_variation, + num_samples: td_samples_ms.len(), + consistency, + }) + } + + /// Check if measurements are consistent enough for reliable analysis + pub fn is_consistent(&self) -> bool { + // Need at least 2 samples for meaningful consistency check + self.num_samples >= TD_SAMPLES_MIN_FOR_STDDEV + && self.consistency >= TD_CONSISTENCY_MIN_THRESHOLD + && self + .coefficient_of_variation + .map_or(true, |cv| cv <= TD_COEFFICIENT_OF_VARIATION_MAX) + } +} + +/// Complete optimal P analysis result +#[derive(Debug, Clone)] +pub struct OptimalPAnalysis { + pub current_p: u32, + pub current_d: Option, + pub recommended_pd_conservative: Option, + pub td_stats: TdStatistics, + pub td_deviation: TdDeviation, + pub td_deviation_percent: f64, + pub noise_level: NoiseLevel, + pub recommendation: PRecommendation, + /// Actual Td target (in ms) used during analysis (from physics) + pub td_target_ms: f64, + /// Actual Td tolerance (in ms) used during analysis (from physics) + pub td_tolerance_ms: f64, +} + +impl OptimalPAnalysis { + /// Analyze optimal P for a given axis + /// + /// # Arguments + /// * `td_samples_ms` - Array of Td measurements from multiple step responses (milliseconds) + /// * `current_p` - Current P gain + /// * `current_d` - Current D gain (optional) + /// * `hf_energy_ratio` - Optional: ratio of D-term energy above DTERM_HF_CUTOFF_HZ (0.0-1.0) + /// * `recommended_pd_conservative` - Optional: recommended P:D ratio from step response (conservative) + /// * `physics_td_target_ms` - Physics-derived (td_target, tolerance) from torque_inertia_profiler + pub fn analyze( + td_samples_ms: &[f64], + current_p: u32, + current_d: Option, + hf_energy_ratio: Option, + recommended_pd_conservative: Option, + physics_td_target_ms: Option<(f64, f64)>, + ) -> Result { + // Calculate Td statistics + let td_stats = TdStatistics::from_samples(td_samples_ms).ok_or_else(|| { + AnalysisError::MissingTdTarget { + message: "Failed to calculate Td statistics from samples.".to_string(), + } + })?; + + // Get target Td — use physics-based value or error + let (td_target_ms, td_tolerance_ms) = + if let Some((phys_target, phys_tol)) = physics_td_target_ms { + (phys_target, phys_tol) + } else { + return Err(AnalysisError::MissingTdTarget { + message: "No physics-derived Td target available. \ + Ensure throttle-punch events were detected (need \ + \u{2265}5 events)." + .to_string(), + }); + }; + + // Defensive check: td_target_ms must be above domain minimum to be physically meaningful + if td_target_ms <= MIN_TD_MS { + return Err(AnalysisError::MissingTdTarget { + message: format!( + "Invalid Td target ({:.3}ms, minimum {:.3}ms) for optimal P analysis. Skipping.", + td_target_ms, MIN_TD_MS + ), + }); + } + + // Calculate deviation from target (safe: td_target_ms validated above) + let td_deviation_percent = ((td_stats.mean_ms - td_target_ms) / td_target_ms) * 100.0; + + // Classify deviation + let td_deviation = if td_deviation_percent > TD_DEVIATION_SIGNIFICANTLY_SLOWER_THRESHOLD { + TdDeviation::SignificantlySlower + } else if td_deviation_percent > TD_DEVIATION_MODERATELY_SLOWER_THRESHOLD { + TdDeviation::ModeratelySlower + } else if td_deviation_percent < TD_DEVIATION_SIGNIFICANTLY_FASTER_THRESHOLD { + TdDeviation::SignificantlyFaster + } else { + TdDeviation::WithinTarget + }; + + // Classify noise level + let noise_level = if let Some(hf_ratio) = hf_energy_ratio { + if hf_ratio < DTERM_HF_ENERGY_MODERATE { + NoiseLevel::Low + } else if hf_ratio < DTERM_HF_ENERGY_THRESHOLD { + NoiseLevel::Moderate + } else { + NoiseLevel::High + } + } else { + NoiseLevel::Unknown + }; + + // Generate recommendation based on Td deviation and noise level + let recommendation = Self::generate_recommendation( + current_p, + &td_deviation, + td_deviation_percent, + &noise_level, + &td_stats, + ); + + Ok(OptimalPAnalysis { + current_p, + current_d, + recommended_pd_conservative, + td_stats, + td_deviation, + td_deviation_percent, + noise_level, + recommendation, + td_target_ms, + td_tolerance_ms, + }) + } + + /// Generate P recommendation based on analysis + fn generate_recommendation( + current_p: u32, + td_deviation: &TdDeviation, + td_deviation_percent: f64, + noise_level: &NoiseLevel, + td_stats: &TdStatistics, + ) -> PRecommendation { + match (td_deviation, noise_level) { + // Case 1: Td significantly slower + low noise = clear headroom to increase P + (TdDeviation::SignificantlySlower, NoiseLevel::Low) => { + let conservative = safe_scaled_p(current_p, P_HEADROOM_MODERATE_MULTIPLIER); + PRecommendation::Increase { + conservative_p: conservative, + reasoning: format!( + "Response is {:.1}% slower than target with low noise levels. \ + P can be increased significantly for faster response.", + td_deviation_percent + ), + } + } + + // Case 2: Td moderately slower + low/moderate noise = modest headroom + (TdDeviation::ModeratelySlower, NoiseLevel::Low | NoiseLevel::Moderate) => { + let conservative = safe_scaled_p(current_p, P_HEADROOM_CONSERVATIVE_MULTIPLIER); + PRecommendation::Increase { + conservative_p: conservative, + reasoning: format!( + "Response is {:.1}% slower than target. Modest P increase recommended.", + td_deviation_percent + ), + } + } + + // Case 2b: Td moderately slower + high noise = investigate + (TdDeviation::ModeratelySlower, NoiseLevel::High) => PRecommendation::Investigate { + issue: format!( + "Response is {:.1}% slower than target despite high noise levels. \ + This suggests mechanical issues (damaged props, loose hardware, motor problems) \ + or incorrect frame class. Inspect aircraft.", + td_deviation_percent + ), + }, + + // Case 3: Td within target + low noise = slight headroom available + (TdDeviation::WithinTarget, NoiseLevel::Low) => { + let conservative = safe_scaled_p(current_p, P_HEADROOM_CONSERVATIVE_MULTIPLIER); + if conservative > current_p { + PRecommendation::Increase { + conservative_p: conservative, + reasoning: "Response time is in target range with low noise. \ + Minor P increase possible if seeking faster response.".to_string(), + } + } else { + PRecommendation::Optimal { + reasoning: format!( + "Response time is in target range ({:.1}ms) with low noise levels. \ + Current P ({}) appears optimal.", + td_stats.mean_ms, current_p + ), + } + } + } + + // Case 4: Td within target + moderate noise = likely optimal + (TdDeviation::WithinTarget, NoiseLevel::Moderate) => PRecommendation::Optimal { + reasoning: format!( + "Response time is in target range ({:.1}ms) with moderate noise levels. \ + Current P ({}) appears optimal for this aircraft.", + td_stats.mean_ms, current_p + ), + }, + + // Case 5: Td within target + high noise = optimal but monitor + (TdDeviation::WithinTarget, NoiseLevel::High) => PRecommendation::Optimal { + reasoning: format!( + "Response time is in target range but noise levels are high. \ + Current P ({}) is at or near physical limits. Monitor motor temperatures.", + current_p + ), + }, + + // Case 6: Td faster than target + high noise = at limit, consider reduction + (TdDeviation::SignificantlyFaster, NoiseLevel::High) => { + let recommended = safe_scaled_p(current_p, P_REDUCTION_MODERATE_MULTIPLIER); + PRecommendation::Decrease { + recommended_p: recommended, + reasoning: format!( + "Response is {:.1}% faster than target with high noise levels. \ + P may be exceeding physical limits. Consider reduction if motors overheat.", + td_deviation_percent + ), + } + } + + // Case 7: Td faster than target + moderate noise = at optimal limit + (TdDeviation::SignificantlyFaster, NoiseLevel::Moderate) => PRecommendation::Optimal { + reasoning: format!( + "Response is {:.1}% faster than target with moderate noise. \ + Current P ({}) is at or near optimal limit for this aircraft.", + td_deviation_percent, current_p + ), + }, + + // Case 8: Td faster than target + low noise = headroom available for P increase + // This is GOOD - faster response + low noise means the prop size might be + // slightly smaller than specified, or the build is exceptionally clean. + // Either way, there's headroom to push P higher if desired. + (TdDeviation::SignificantlyFaster, NoiseLevel::Low) => { + let conservative = safe_scaled_p(current_p, P_HEADROOM_CONSERVATIVE_MULTIPLIER); + if conservative > current_p { + PRecommendation::Increase { + conservative_p: conservative, + reasoning: format!( + "Response is {:.1}% faster than target with low noise levels. \ + This indicates excellent build quality with headroom for P increase. \ + Note: Verify prop size is correct (may be smaller than specified).", + td_deviation_percent + ), + } + } else { + PRecommendation::Optimal { + reasoning: format!( + "Response is {:.1}% faster than target with low noise. \ + Current P ({}) is optimal. Excellent build quality or prop size may differ from spec.", + td_deviation_percent, current_p + ), + } + } + } + + // Case 9: Td significantly slower + moderate/high noise = investigate + (TdDeviation::SignificantlySlower, NoiseLevel::Moderate | NoiseLevel::High) => { + PRecommendation::Investigate { + issue: format!( + "Response is {:.1}% slower than target despite moderate/high noise. \ + This suggests mechanical issues (damaged props, loose hardware, \ + motor problems) or incorrect frame class. Inspect aircraft.", + td_deviation_percent + ), + } + } + + // Case 10: Unknown noise level - provide Td-only recommendation + (_, NoiseLevel::Unknown) => match td_deviation { + TdDeviation::SignificantlySlower | TdDeviation::ModeratelySlower => { + let conservative = + safe_scaled_p(current_p, P_HEADROOM_CONSERVATIVE_MULTIPLIER); + PRecommendation::Increase { + conservative_p: conservative, + reasoning: format!( + "Response is {:.1}% slower than target. P increase recommended, \ + but monitor motor temperatures (D-term data unavailable for noise analysis).", + td_deviation_percent + ), + } + } + TdDeviation::WithinTarget => PRecommendation::Optimal { + reasoning: format!( + "Response time ({:.1}ms) is in target range. Current P ({}) appears appropriate, \ + but monitor motor temperatures (D-term data unavailable for noise analysis).", + td_stats.mean_ms, current_p + ), + }, + TdDeviation::SignificantlyFaster => PRecommendation::Optimal { + reasoning: format!( + "Response is {:.1}% faster than target. Current P ({}) may be at limits, \ + monitor motor temperatures (D-term data unavailable for noise analysis).", + td_deviation_percent, current_p + ), + }, + }, + } + } + + /// Format analysis as human-readable console output + pub fn format_console_output(&self, axis_name: &str) -> String { + let target_display = format!("{:.1}±{:.1}ms", self.td_target_ms, self.td_tolerance_ms); + let mut output = String::new(); + + // Compact header - axis name and basic info + output.push_str(&format!( + "{}: Td={:.1}ms (target {}, {:+.0}% dev, windows={}), Noise={}, Consistency={:.0}%\n", + axis_name, + self.td_stats.mean_ms, + target_display, + self.td_deviation_percent, + self.td_stats.num_samples, + self.noise_level.name(), + self.td_stats.consistency * 100.0 + )); + + // Warning for low consistency (inline) + if !self.td_stats.is_consistent() { + let cv_percent = self + .td_stats + .coefficient_of_variation + .map_or(0.0, |cv| cv * 100.0); + output.push_str(&format!( + " ⚠ Low consistency (CV={:.1}%) — unreliable (>{:.0}%)\n", + cv_percent, + TD_COEFFICIENT_OF_VARIATION_MAX * 100.0 + )); + } + + // Compact recommendation + output.push_str(&format!(" Current P={}\n", self.current_p)); + + match &self.recommendation { + PRecommendation::Optimal { reasoning } => { + output.push_str(" → Optimal (no change recommended)\n"); + output.push_str(&format!(" {}\n", reasoning)); + } + PRecommendation::Increase { + conservative_p, + reasoning, + } => { + output.push_str(" → Increase recommended:\n"); + + // Calculate P delta + let conservative_delta = *conservative_p as i32 - self.current_p as i32; + + // Show P recommendation (conservative only for simplicity) + output.push_str(&format!( + " Conservative: P≈{} ({:+})", + conservative_p, conservative_delta + )); + + // Add D recommendation: prefer step-response P:D, fall back to current P:D. + let effective_pd = self.recommended_pd_conservative.or_else(|| { + self.current_d + .filter(|&d| d > 0) + .map(|d| self.current_p as f64 / d as f64) + }); + if let (Some(current_d), Some(rec_pd)) = (self.current_d, effective_pd) { + if rec_pd > 0.0 && current_d > 0 { + let conservative_d = ((*conservative_p as f64) / rec_pd).round() as u32; + let conservative_d_delta = conservative_d as i32 - current_d as i32; + output.push_str(&format!( + ", D≈{} ({:+})", + conservative_d, conservative_d_delta + )); + } + } + output.push('\n'); + + output.push_str(&format!(" {}\n", reasoning)); + } + PRecommendation::Decrease { + recommended_p, + reasoning, + } => { + output.push_str(" → Decrease recommended:\n"); + let decrease_delta = *recommended_p as i32 - self.current_p as i32; + output.push_str(&format!(" P≈{} ({:+})", recommended_p, decrease_delta)); + + // Add D recommendation: prefer step-response P:D, fall back to current P:D. + let effective_pd = self.recommended_pd_conservative.or_else(|| { + self.current_d + .filter(|&d| d > 0) + .map(|d| self.current_p as f64 / d as f64) + }); + if let (Some(current_d), Some(rec_pd)) = (self.current_d, effective_pd) { + if rec_pd > 0.0 && current_d > 0 { + let recommended_d = ((*recommended_p as f64) / rec_pd).round() as u32; + let d_delta = recommended_d as i32 - current_d as i32; + output.push_str(&format!(", D≈{} ({:+})", recommended_d, d_delta)); + } + } + output.push('\n'); + + output.push_str(&format!(" {}\n", reasoning)); + } + PRecommendation::Investigate { issue } => { + output.push_str(" → ⚠ INVESTIGATION RECOMMENDED\n"); + output.push_str(&format!(" {}\n", issue)); + } + } + + output + } +} diff --git a/src/data_analysis/spectral_analysis.rs b/src/data_analysis/spectral_analysis.rs index 8dcc768..46eea9f 100644 --- a/src/data_analysis/spectral_analysis.rs +++ b/src/data_analysis/spectral_analysis.rs @@ -4,6 +4,7 @@ use ndarray::Array1; use num_complex::Complex64; use std::error::Error; +use crate::constants::PSD_EPSILON; use crate::data_analysis::{calc_step_response, fft_utils}; /// Configuration for Welch's method spectral analysis @@ -330,3 +331,70 @@ pub fn coherence( Ok(coh) } + +/// Calculate high-frequency energy ratio for D-term noise analysis +/// +/// Returns the ratio of energy above the specified high-frequency cutoff to total energy. +/// The parameter `hf_cutoff` (in Hz) defines the high-frequency cutoff used by this function. +/// To use the project's default cutoff, pass the constant `DTERM_HF_CUTOFF_HZ` defined in +/// `crate::constants` as the `hf_cutoff` argument. This function is used by the Optimal P +/// estimation pipeline to assess high-frequency noise headroom. +/// +/// # Arguments +/// * `data` - D-term time series data +/// * `sample_rate` - Sample rate in Hz +/// * `hf_cutoff` - High-frequency cutoff threshold in Hz (must be > 0 and < sample_rate/2) +/// +/// # Returns +/// * `Some(ratio)` - Ratio of HF energy (0.0 to 1.0) if analysis succeeds +/// * `None` - If data is insufficient, hf_cutoff invalid, or analysis fails +pub fn calculate_hf_energy_ratio(data: &[f32], sample_rate: f64, hf_cutoff: f64) -> Option { + if data.is_empty() || sample_rate <= 0.0 { + return None; + } + + // Validate high-frequency cutoff: must be positive and below Nyquist (sample_rate / 2) + let nyquist = sample_rate / 2.0; + if hf_cutoff.is_nan() || hf_cutoff <= 0.0 || hf_cutoff >= nyquist { + eprintln!("Warning: Invalid hf_cutoff {} Hz (must be >0 and < Nyquist {} Hz). Skipping HF energy ratio.", hf_cutoff, nyquist); + return None; + } + + // Use Welch's method for robust PSD estimation + let config = WelchConfig::default(); + let psd = match welch_psd(data, sample_rate, Some(config.clone())) { + Ok(psd) => psd, + Err(e) => { + eprintln!( + "Warning: Welch PSD calculation failed (data_len={}, sample_rate={} Hz, config={:?}): {}. Skipping HF energy ratio.", + data.len(), + sample_rate, + config, + e + ); + return None; + } + }; + + if psd.is_empty() { + return None; + } + + // Calculate total energy and HF energy + let mut total_energy = 0.0; + let mut hf_energy = 0.0; + + for &(freq, power) in &psd { + total_energy += power; + if freq >= hf_cutoff { + hf_energy += power; + } + } + + // Return ratio if total energy is significant + if total_energy > PSD_EPSILON { + Some((hf_energy / total_energy).clamp(0.0, 1.0)) + } else { + None + } +} diff --git a/src/data_analysis/torque_inertia_profiler.rs b/src/data_analysis/torque_inertia_profiler.rs new file mode 100644 index 0000000..175a159 --- /dev/null +++ b/src/data_analysis/torque_inertia_profiler.rs @@ -0,0 +1,245 @@ +// src/data_analysis/torque_inertia_profiler.rs +// +// Torque-Inertia Profiler +// +// Derives aircraft-specific Td targets from throttle-punch events in flight logs. +// Replaces the empirical frame-class lookup table with values measured from actual +// aircraft dynamics. +// +// Algorithm: +// 1. Detect throttle-punch events: setpoint[3] increases >= THROTTLE_PUNCH_MIN_DELTA +// within THROTTLE_PUNCH_WINDOW_MS. +// 2. For each punch: measure peak |d(gyro[axis])/dt| in the response window. +// 3. Normalize by throttle delta: torque_inertia_ratio = peak_alpha / cmd_delta_normalized +// 4. Aggregate across all logs in the aircraft group; compute median + spread. +// 5. Td_target_ms = TORQUE_PROFILER_TD_CALC_K / sqrt((P / P_SCALE) * torque_inertia_ratio) +// +// The TORQUE_PROFILER_P_SCALE constant accounts for the unit mismatch between raw +// Betaflight/EmuFlight P gain values and the physics formula. It may require empirical +// calibration against known-good aircraft. + +use crate::axis_names::AXIS_COUNT; +use crate::constants::{ + OPTIMAL_P_SECONDS_TO_MS_MULTIPLIER, THROTTLE_COMMAND_SCALE, THROTTLE_PUNCH_MIN_DELTA, + THROTTLE_PUNCH_WINDOW_MS, THROTTLE_RESPONSE_WINDOW_MS, + TORQUE_PROFILER_MIN_CMD_DELTA_NORMALIZED, TORQUE_PROFILER_MIN_DT_S, TORQUE_PROFILER_MIN_EVENTS, + TORQUE_PROFILER_P_SCALE, TORQUE_PROFILER_SETTLE_MS, TORQUE_PROFILER_TD_CALC_K, +}; +use crate::data_input::log_data::LogRowData; + +/// Per-axis profiling result derived from aggregated throttle-punch events. +#[derive(Debug, Clone, Default)] +pub struct AxisProfile { + /// Median torque-to-inertia ratio [deg/s² per normalized throttle unit (0–1)] + pub torque_inertia_ratio: Option, + /// Spread as a fraction of the median (half inter-quartile range / median). + /// Used for tolerance estimation. None if fewer than 4 events. + pub ratio_spread_fraction: Option, + /// Number of valid throttle-punch events used to compute this profile. + pub event_count: usize, +} + +impl AxisProfile { + fn from_ratios(mut ratios: Vec) -> Self { + let n = ratios.len(); + if n == 0 { + return Self::default(); + } + ratios.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal)); + + let median = if n % 2 == 0 { + (ratios[n / 2 - 1] + ratios[n / 2]) / 2.0 + } else { + ratios[n / 2] + }; + + if median <= 0.0 { + return Self::default(); + } + + // Half-IQR normalised by median as a spread estimate. + let ratio_spread_fraction = if n >= 4 { + let q1 = ratios[n / 4]; + let q3 = ratios[(3 * n) / 4]; + let half_iqr = (q3 - q1) / 2.0; + Some(half_iqr / median) + } else { + None + }; + + AxisProfile { + torque_inertia_ratio: Some(median), + ratio_spread_fraction, + event_count: n, + } + } + + /// Compute the physics-derived Td target and tolerance for this axis given the + /// current P gain. + /// + /// Returns None when fewer than TORQUE_PROFILER_MIN_EVENTS events were found or + /// the profiling data is otherwise insufficient. + pub fn td_target_ms(&self, current_p: u32) -> Option<(f64, f64)> { + if self.event_count < TORQUE_PROFILER_MIN_EVENTS { + return None; + } + let ratio = self.torque_inertia_ratio?; + if ratio <= 0.0 || current_p == 0 { + return None; + } + let p_effective = (current_p as f64) / TORQUE_PROFILER_P_SCALE; + let discriminant = p_effective * ratio; + if discriminant <= 0.0 { + return None; + } + let omega_n = discriminant.sqrt(); + let td_ms = (TORQUE_PROFILER_TD_CALC_K / omega_n) + * crate::constants::TORQUE_PROFILER_ACHIEVABILITY_FACTOR; + if !td_ms.is_finite() || td_ms <= 0.0 { + return None; + } + // Propagate ratio spread to tolerance; default 25 % when spread is unknown. + let tolerance_ms = self + .ratio_spread_fraction + .map(|f| td_ms * f * 0.5) + .unwrap_or(td_ms * 0.25) + .max(1.0); // minimum 1 ms + Some((td_ms, tolerance_ms)) + } +} + +/// Per-aircraft profile aggregated across all logs in a group. +#[derive(Debug, Clone, Default)] +pub struct AircraftProfile { + /// Per-axis profiles (Roll=0, Pitch=1, Yaw=2). + pub axes: [AxisProfile; AXIS_COUNT], +} + +impl AircraftProfile { + /// Build a profile from pre-collected per-axis ratio vectors. + pub fn from_axis_ratios(axis_ratios: [Vec; AXIS_COUNT]) -> Self { + let [r0, r1, r2] = axis_ratios; + AircraftProfile { + axes: [ + AxisProfile::from_ratios(r0), + AxisProfile::from_ratios(r1), + AxisProfile::from_ratios(r2), + ], + } + } + + /// Returns a human-readable summary for console display. + pub fn summary(&self) -> String { + let mut s = String::new(); + let axis_names = crate::axis_names::AXIS_NAMES; + for (i, (profile, name)) in self + .axes + .iter() + .zip(axis_names.iter()) + .enumerate() + .take(crate::axis_names::ROLL_PITCH_AXIS_COUNT) + { + if let Some(ratio) = profile.torque_inertia_ratio { + s.push_str(&format!( + " {}: torque_ratio={:.0} deg/s²/cmd ({} events)\n", + name, ratio, profile.event_count + )); + } else { + s.push_str(&format!( + " {}: insufficient events ({}/{} required)\n", + name, profile.event_count, TORQUE_PROFILER_MIN_EVENTS + )); + } + let _ = i; // suppress unused warning + } + s + } +} + +/// Detect throttle-punch events in a single log and return the per-axis +/// torque-inertia ratio estimates. +/// +/// A throttle-punch is defined as `setpoint[3]` increasing by at least +/// `THROTTLE_PUNCH_MIN_DELTA` (in 0–1000 units) within `THROTTLE_PUNCH_WINDOW_MS`. +/// For each punch the peak angular acceleration in the following +/// `THROTTLE_RESPONSE_WINDOW_MS` window is divided by the normalised command delta +/// to yield the torque-inertia ratio for each axis. +pub fn extract_punch_ratios(log_data: &[LogRowData], sample_rate: f64) -> [Vec; AXIS_COUNT] { + let mut axis_ratios: [Vec; AXIS_COUNT] = std::array::from_fn(|_| Vec::new()); + + if log_data.len() < 10 || sample_rate <= 0.0 { + return axis_ratios; + } + + let punch_window = ((THROTTLE_PUNCH_WINDOW_MS / OPTIMAL_P_SECONDS_TO_MS_MULTIPLIER) + * sample_rate) + .ceil() as usize; + let response_window = ((THROTTLE_RESPONSE_WINDOW_MS / OPTIMAL_P_SECONDS_TO_MS_MULTIPLIER) + * sample_rate) + .ceil() as usize; + let punch_window = punch_window.max(2); + let response_window = response_window.max(5); + + let n = log_data.len(); + let mut i = 0usize; + + while i + punch_window + response_window < n { + let t_start = log_data[i].setpoint[3]; + let t_end = log_data[i + punch_window].setpoint[3]; + + if let (Some(throttle_start), Some(throttle_end)) = (t_start, t_end) { + let delta = throttle_end - throttle_start; + let cmd_delta_normalized = delta / THROTTLE_COMMAND_SCALE; + + if delta >= THROTTLE_PUNCH_MIN_DELTA + && cmd_delta_normalized >= TORQUE_PROFILER_MIN_CMD_DELTA_NORMALIZED + { + // Punch detected at sample i. + let settle_samples = + (TORQUE_PROFILER_SETTLE_MS / OPTIMAL_P_SECONDS_TO_MS_MULTIPLIER * sample_rate) + .ceil() as usize; + let resp_start = (i + punch_window + settle_samples).min(n - 1); + let resp_end = (resp_start + response_window).min(n - 1); + + if resp_end > resp_start + 2 { + // Axis 2 (Yaw) is collected here for diagnostic completeness but + // is not used in optimal-P analysis (Roll/Pitch only). + for (axis, axis_ratio_vec) in axis_ratios.iter_mut().enumerate() { + let mut peak_alpha: f64 = 0.0; + for j in resp_start..resp_end { + if let (Some(g0), Some(g1), Some(t0), Some(t1)) = ( + log_data[j].gyro[axis], + log_data[j + 1].gyro[axis], + log_data[j].time_sec, + log_data[j + 1].time_sec, + ) { + let dt = t1 - t0; + if dt > TORQUE_PROFILER_MIN_DT_S { + let alpha = (g1 - g0).abs() / dt; + if alpha > peak_alpha { + peak_alpha = alpha; + } + } + } + } + if peak_alpha > 0.0 { + let ratio = peak_alpha / cmd_delta_normalized; + if ratio.is_finite() && ratio > 0.0 { + axis_ratio_vec.push(ratio); + } + } + } + } + + // Advance past the response window. + i += punch_window + response_window; + continue; + } + } + i += 1; + } + + axis_ratios +} + +// src/data_analysis/torque_inertia_profiler.rs diff --git a/src/main.rs b/src/main.rs index 6615893..f09fb92 100644 --- a/src/main.rs +++ b/src/main.rs @@ -11,7 +11,7 @@ mod plot_framework; mod plot_functions; mod types; -use std::collections::HashSet; +use std::collections::{BTreeMap, HashSet}; use std::env; use std::error::Error; use std::fs; @@ -19,6 +19,7 @@ use std::path::{Path, PathBuf}; use ndarray::Array1; +use crate::data_analysis::torque_inertia_profiler::{extract_punch_ratios, AircraftProfile}; use crate::types::StepResponseResults; // Build version string from git info with fallbacks for builds without vergen metadata @@ -92,6 +93,16 @@ impl PlotConfig { } } +// Analysis options struct to group related analysis parameters +#[derive(Debug, Clone, Copy)] +struct AnalysisOptions { + pub setpoint_threshold: f64, + pub show_legend: bool, + pub debug_mode: bool, + pub show_butterworth: bool, + pub estimate_optimal_p: bool, +} + use crate::constants::{ DEFAULT_SETPOINT_THRESHOLD, EXCLUDE_END_S, EXCLUDE_START_S, FRAME_LENGTH_S, }; @@ -366,6 +377,7 @@ fn print_usage_and_exit(program_name: &str) { eprintln!( " --dps : Deg/s threshold for detailed step response plots (positive number)." ); + eprintln!(" --estimate-optimal-p: Enable optimal P estimation from throttle-punch dynamics. Requires .headers.csv; skips P estimation if absent."); eprintln!(); eprintln!("=== GENERAL ==="); eprintln!(); @@ -375,16 +387,141 @@ fn print_usage_and_exit(program_name: &str) { std::process::exit(1); } -#[allow(clippy::too_many_arguments)] +/// Extract an aircraft grouping key from a file path. +/// +/// Strips the date-time portion (`_YYYYMMDD_HHMMSS`) so files from the same aircraft +/// across multiple sessions share one key. When the craft name follows the timestamp +/// (e.g. the standard Betaflight naming scheme), it is appended to the prefix so that +/// different craft logged under the same generic prefix remain distinct. +/// +/// Examples: +/// `EMUF_BLACKBOX_LOG_FOXEERF722V4_426_20240406_132335_notes.19.csv` +/// → `EMUF_BLACKBOX_LOG_FOXEERF722V4_426` (craft name precedes date) +/// `BTFL_BLACKBOX_LOG_20250517_130413_STELLARH7DEV.02.csv` +/// → `BTFL_BLACKBOX_LOG_STELLARH7DEV` (generic prefix; craft name appended from suffix) +/// +/// Files without a date pattern are treated as unique aircraft (full stem as key). +fn extract_aircraft_key(path: &Path) -> String { + let stem = path.file_stem().and_then(|s| s.to_str()).unwrap_or(""); + + // Scan for the pattern _YYYYMMDD_HHMMSS (8-digit date, underscore, 6-digit time). + let bytes = stem.as_bytes(); + let min_len = 16; // '_' + 8 digits + '_' + 6 digits + if bytes.len() >= min_len { + for i in 0..=(bytes.len() - min_len) { + if bytes[i] == b'_' + && bytes[i + 1..i + 9].iter().all(|&b| b.is_ascii_digit()) + && bytes[i + 9] == b'_' + && bytes[i + 10..i + 16].iter().all(|&b| b.is_ascii_digit()) + { + // i == 0 would give an empty key; fall through to full stem. + if i > 0 { + let prefix = &stem[..i]; + let after_datetime = &stem[(i + 16)..]; + // When the prefix is a generic firmware placeholder (ends with + // "BLACKBOX_LOG"), the craft name sits in the suffix; include it + // so files from different craft don't collapse into one group. + if !after_datetime.is_empty() && prefix.ends_with("BLACKBOX_LOG") { + let craft = after_datetime.strip_prefix('_').unwrap_or(after_datetime); + // Strip trailing log-number suffix (e.g. ".02", ".19"). + let craft = if let Some(dot) = craft.rfind('.') { + let tail = &craft[dot + 1..]; + if tail.chars().all(|c| c.is_ascii_digit()) { + &craft[..dot] + } else { + craft + } + } else { + craft + }; + if !craft.is_empty() { + return format!("{}_{}", prefix, craft); + } + } + return prefix.to_string(); + } + } + } + } + + // No date pattern found — use full stem. + stem.to_string() +} + +/// Group a list of file paths by aircraft key. +/// +/// Returns a `BTreeMap` (sorted by key) mapping each aircraft key to its files. +fn group_files_by_aircraft(input_files: &[String]) -> BTreeMap> { + let mut groups: BTreeMap> = BTreeMap::new(); + for file in input_files { + let key = extract_aircraft_key(Path::new(file)); + groups.entry(key).or_default().push(file.clone()); + } + groups +} + +/// Parse all files in a group and collect torque-inertia ratio estimates. +/// +/// This is the Phase 1 profiling pass. Each file is parsed minimally and the +/// punch-event ratios are aggregated into a single `AircraftProfile`. +fn profile_aircraft_group(files: &[String], debug_mode: bool) -> AircraftProfile { + let mut all_axis_ratios: [Vec; crate::axis_names::AXIS_COUNT] = + std::array::from_fn(|_| Vec::new()); + let mut files_profiled: usize = 0; + + for file_str in files { + let path = Path::new(file_str); + match parse_log_file(path, debug_mode) { + Ok((log_data, Some(sr), ..)) => { + let ratios = extract_punch_ratios(&log_data, sr); + let total_events: usize = ratios.iter().map(|v| v.len()).sum(); + for (axis, axis_ratio_vec) in all_axis_ratios.iter_mut().enumerate() { + axis_ratio_vec.extend_from_slice(&ratios[axis]); + } + if debug_mode { + println!( + " [torque-profile] {}: {} throttle-punch events across all axes", + path.file_name() + .and_then(|s| s.to_str()) + .unwrap_or(file_str), + total_events + ); + } + files_profiled += 1; + } + Ok((_, None, ..)) => { + if debug_mode { + eprintln!(" [torque-profile] Skipping {} (no sample rate)", file_str); + } + } + Err(e) => { + if debug_mode { + eprintln!(" [torque-profile] Failed to parse {}: {}", file_str, e); + } + } + } + } + + if debug_mode { + println!( + " [torque-profile] Profiled {} file(s). Events: Roll={}, Pitch={}, Yaw={}", + files_profiled, + all_axis_ratios[0].len(), + all_axis_ratios[1].len(), + all_axis_ratios[2].len() + ); + } + + AircraftProfile::from_axis_ratios(all_axis_ratios) +} + fn process_file( input_file_str: &str, - setpoint_threshold: f64, - show_legend: bool, use_dir_prefix: bool, output_dir: Option<&Path>, - debug_mode: bool, - show_butterworth: bool, plot_config: PlotConfig, + analysis_opts: AnalysisOptions, + aircraft_profile: &AircraftProfile, ) -> Result<(), Box> { // --- Setup paths and names --- let input_path = Path::new(input_file_str); @@ -436,7 +573,7 @@ fn process_file( gyro_unfilt_header_found, debug_header_found, header_metadata, - ) = match parse_log_file(input_path, debug_mode) { + ) = match parse_log_file(input_path, analysis_opts.debug_mode) { Ok(data) => data, Err(e) => { eprintln!("Error parsing log file {input_file_str}: {e}"); @@ -639,6 +776,12 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." if sample_rate.is_some() { println!("\n--- Step Response Analysis & P:D Ratio Recommendations ---"); println!("NOTE: These are STARTING POINTS based on step response analysis."); + println!(" These recommendations focus on D-term tuning (P:D ratio)."); + if analysis_opts.estimate_optimal_p { + println!( + " See 'Optimal P Estimation' below for P gain magnitude recommendations." + ); + } println!(" Always test in a safe environment. Conservative = safer first step."); println!(" Moderate = for experienced pilots (test carefully to avoid hot motors)."); println!(); @@ -973,6 +1116,161 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." println!(); } + // Optimal P Estimation Analysis (if enabled) + // Store results for both console output and PNG overlay + let mut optimal_p_analyses: [Option< + crate::data_analysis::optimal_p_estimation::OptimalPAnalysis, + >; crate::axis_names::AXIS_COUNT] = std::array::from_fn(|_| None); + let mut optimal_p_skip_reasons: [Option; crate::axis_names::AXIS_COUNT] = + std::array::from_fn(|_| None); + + if analysis_opts.estimate_optimal_p { + if let Some(sr) = sample_rate { + println!("\n--- Optimal P Estimation ---"); + println!("Td target: physics-derived from throttle-punch events in log group."); + println!(); + + for axis_index in 0..crate::axis_names::ROLL_PITCH_AXIS_COUNT { + // Only Roll (0) and Pitch (1) - Yaw excluded by ROLL_PITCH_AXIS_COUNT + let axis_name = crate::axis_names::AXIS_NAMES[axis_index]; + + if let Some((response_time, valid_stacked_responses, valid_window_max_setpoints)) = + &step_response_calculation_results[axis_index] + { + if valid_stacked_responses.shape()[0] > 0 && !response_time.is_empty() { + // Warn when inputs are too gentle to produce reliable step-response data + let max_sp = valid_window_max_setpoints + .iter() + .cloned() + .fold(0.0_f32, f32::max); + if max_sp < crate::constants::LOW_AUTHORITY_SETPOINT_THRESHOLD_DEG_S { + println!( + " ⚠ {axis_name}: [LOW AUTHORITY] max={:.0}dps — recommendations unreliable", + max_sp + ); + } + + // Collect individual Td samples from each valid response window + let mut td_samples_ms: Vec = Vec::new(); + + for window_idx in 0..valid_stacked_responses.shape()[0] { + let response = valid_stacked_responses.row(window_idx); + let response_f64: Vec = + response.iter().map(|&x| x as f64).collect(); + let response_arr = Array1::from_vec(response_f64); + + if let Some(td_seconds) = + calc_step_response::calculate_delay_time(&response_arr, sr) + { + td_samples_ms.push( + td_seconds + * crate::constants::OPTIMAL_P_SECONDS_TO_MS_MULTIPLIER, + ); + } + } + + if td_samples_ms.is_empty() { + println!(" No valid Td measurements for {axis_name}. Skipping optimal P analysis."); + optimal_p_skip_reasons[axis_index] = + Some("No valid Td measurements".to_string()); + continue; + } + + // Get current P gain + let current_p = if axis_index == 0 { + pid_metadata.roll.p + } else { + pid_metadata.pitch.p + }; + + // Get current D gain + let current_d = if axis_index == 0 { + pid_metadata.roll.d + } else { + pid_metadata.pitch.d + }; + + if let Some(p_gain) = current_p { + // Calculate HF noise energy from D-term data if available + let hf_energy_ratio: Option = { + // Collect D-term data for this axis from the log + let d_term_data: Vec = all_log_data + .iter() + .filter_map(|row| row.d_term[axis_index].map(|v| v as f32)) + .collect(); + + // Only analyze if we have sufficient D-term data and sample rate + if !d_term_data.is_empty() + && d_term_data.len() + >= crate::constants::OPTIMAL_P_MIN_DTERM_SAMPLES + { + crate::data_analysis::spectral_analysis::calculate_hf_energy_ratio( + &d_term_data, + sr, + crate::constants::DTERM_HF_CUTOFF_HZ, + ) + } else { + None + } + }; + + // Compute physics-derived Td target for this axis + // using the group's torque-inertia profile and current P gain. + let physics_td = aircraft_profile.axes[axis_index].td_target_ms(p_gain); + + if physics_td.is_none() { + let events = aircraft_profile.axes[axis_index].event_count; + if events < crate::constants::TORQUE_PROFILER_MIN_EVENTS { + let msg = format!( + "SKIPPED: insufficient throttle dynamics ({} events, need >={})", + events, crate::constants::TORQUE_PROFILER_MIN_EVENTS + ); + println!(" {}: {}.", axis_name, msg); + println!(" Provide logs with more throttle variation or fly deliberate punch sequences."); + optimal_p_skip_reasons[axis_index] = Some(msg); + } else { + let msg = + "SKIPPED: could not compute Td target from profiling data" + .to_string(); + println!(" {}: {}.", axis_name, msg); + optimal_p_skip_reasons[axis_index] = Some(msg); + } + continue; + } + + // Perform optimal P analysis + match crate::data_analysis::optimal_p_estimation::OptimalPAnalysis::analyze( + &td_samples_ms, + p_gain, + current_d, + hf_energy_ratio, + recommended_pd_conservative[axis_index], + physics_td, + ) { + Ok(analysis) => { + // Print console output + println!("{}", analysis.format_console_output(axis_name)); + // Store for PNG overlay (move instead of clone) + optimal_p_analyses[axis_index] = Some(analysis); + } + Err(e) => { + // Log the error for user visibility + eprintln!("Warning: {}", e); + optimal_p_skip_reasons[axis_index] = Some(e.to_string()); + } + } + } else { + let msg = "SKIPPED: P gain not available".to_string(); + println!(" {axis_name}: {msg}"); + optimal_p_skip_reasons[axis_index] = Some(msg); + } + } + } + } + println!(); + } + } + // Create RAII guard BEFORE changing directory if needed let _cwd_guard = if let Some(output_dir) = output_dir { // Create guard to save current directory BEFORE changing it @@ -991,25 +1289,53 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." let pid_context = PidContext::new(sample_rate, pid_metadata, root_name_string.clone()); if plot_config.step_response { + // Group related parameters into structs for cleaner API + use crate::plot_functions::plot_step_response::{ + ConservativeRecommendations, CurrentPeakAndRatios, ModerateRecommendations, + OptimalPConfig, PdRecommendations, PlotDisplayConfig, + }; + + let current = CurrentPeakAndRatios { + peak_values, + pd_ratios: current_pd_ratios, + assessments, + }; + + let conservative = ConservativeRecommendations(PdRecommendations { + pd_ratios: recommended_pd_conservative, + d_values: recommended_d_conservative, + d_min_values: recommended_d_min_conservative, + d_max_values: recommended_d_max_conservative, + }); + + let moderate = ModerateRecommendations(PdRecommendations { + pd_ratios: recommended_pd_aggressive, + d_values: recommended_d_aggressive, + d_min_values: recommended_d_min_aggressive, + d_max_values: recommended_d_max_aggressive, + }); + + let display = PlotDisplayConfig { + has_nonzero_f_term: has_nonzero_f_term_data, + setpoint_threshold: analysis_opts.setpoint_threshold, + show_legend: analysis_opts.show_legend, + }; + + let optimal_p = OptimalPConfig { + analyses: optimal_p_analyses, + skip_reasons: optimal_p_skip_reasons, + }; + plot_step_response( &step_response_calculation_results, &root_name_string, sample_rate, - &has_nonzero_f_term_data, - setpoint_threshold, - show_legend, &pid_context.pid_metadata, - &peak_values, - ¤t_pd_ratios, - &assessments, - &recommended_pd_conservative, - &recommended_d_conservative, - &recommended_d_min_conservative, - &recommended_d_max_conservative, - &recommended_pd_aggressive, - &recommended_d_aggressive, - &recommended_d_min_aggressive, - &recommended_d_max_aggressive, + ¤t, + &conservative, + &moderate, + &display, + &optimal_p, )?; } @@ -1063,7 +1389,7 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." &root_name_string, sample_rate, Some(&header_metadata), - show_butterworth, + analysis_opts.show_butterworth, using_debug_fallback, debug_mode_label, )?; @@ -1075,7 +1401,7 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." &root_name_string, sample_rate, Some(&header_metadata), - debug_mode, + analysis_opts.debug_mode, using_debug_fallback, debug_mode_label, )?; @@ -1087,7 +1413,7 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." &root_name_string, sample_rate, Some(&header_metadata), - show_butterworth, + analysis_opts.show_butterworth, using_debug_fallback, debug_mode_label, )?; @@ -1114,7 +1440,12 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." " For normal flight log analysis, use spectrum plots (default behavior) instead." ); eprintln!(); - plot_bode_analysis(&all_log_data, &root_name_string, sample_rate, debug_mode)?; + plot_bode_analysis( + &all_log_data, + &root_name_string, + sample_rate, + analysis_opts.debug_mode, + )?; } if plot_config.psd_db_heatmap { @@ -1177,6 +1508,7 @@ fn main() -> Result<(), Box> { let mut bode_requested = false; let mut pid_requested = false; let mut recursive = false; + let mut estimate_optimal_p = false; let mut version_flag_set = false; @@ -1244,6 +1576,8 @@ fn main() -> Result<(), Box> { } else if arg == "--pid" { has_only_flags = true; pid_requested = true; + } else if arg == "--estimate-optimal-p" { + estimate_optimal_p = true; } else if arg.starts_with("--") { eprintln!("Error: Unknown option '{arg}'"); print_usage_and_exit(program_name); @@ -1328,29 +1662,54 @@ fn main() -> Result<(), Box> { } } + // Construct AnalysisOptions once before the loop (Copy type, reusable across all files) + let analysis_opts = AnalysisOptions { + setpoint_threshold, + show_legend, + debug_mode, + show_butterworth, + estimate_optimal_p, + }; + + // Group input files by aircraft key for two-phase processing. + // Phase 1 (profiling) aggregates throttle-punch events across all logs for each group. + // Phase 2 (processing) runs the standard per-file analysis with the group's Td target. + let grouped_files = group_files_by_aircraft(&input_files); + let mut overall_success = true; - for input_file_str in &input_files { - // Determine the actual output directory for this file - let actual_output_dir = match &output_dir { - None => { - // No --output-dir specified, use input file's directory (source folder) - Path::new(input_file_str).parent() - } - Some(dir) => Some(Path::new(dir)), // --output-dir with specific directory + for (craft_key, group_files) in &grouped_files { + // Phase 1: torque-inertia profiling across all files in the group. + let aircraft_profile = if analysis_opts.estimate_optimal_p { + println!( + "\n--- Torque-Inertia Profiling: '{}' ({} file(s)) ---", + craft_key, + group_files.len() + ); + let profile = profile_aircraft_group(group_files, debug_mode); + print!("{}", profile.summary()); + profile + } else { + AircraftProfile::default() }; - if let Err(e) = process_file( - input_file_str, - setpoint_threshold, - show_legend, - use_dir_prefix_for_root_name, - actual_output_dir, - debug_mode, - show_butterworth, - plot_config, - ) { - eprintln!("An error occurred while processing {input_file_str}: {e}"); - overall_success = false; + // Phase 2: process each file in the group. + for input_file_str in group_files { + let actual_output_dir = match &output_dir { + None => Path::new(input_file_str).parent(), + Some(dir) => Some(Path::new(dir)), + }; + + if let Err(e) = process_file( + input_file_str, + use_dir_prefix_for_root_name, + actual_output_dir, + plot_config, + analysis_opts, + &aircraft_profile, + ) { + eprintln!("An error occurred while processing {input_file_str}: {e}"); + overall_success = false; + } } } diff --git a/src/plot_functions/plot_step_response.rs b/src/plot_functions/plot_step_response.rs index 048f69f..f73cef2 100644 --- a/src/plot_functions/plot_step_response.rs +++ b/src/plot_functions/plot_step_response.rs @@ -4,39 +4,95 @@ use ndarray::{s, Array1, Array2}; use plotters::style::RGBColor; use std::error::Error; -use crate::axis_names::AXIS_NAMES; +use crate::axis_names::{AXIS_COUNT, AXIS_NAMES}; use crate::constants::{ + COLOR_OPTIMAL_P_DIVIDER, COLOR_OPTIMAL_P_HEADER, COLOR_OPTIMAL_P_RECOMMENDATION, + COLOR_OPTIMAL_P_SKIP, COLOR_OPTIMAL_P_TEXT, COLOR_OPTIMAL_P_WARNING, COLOR_STEP_RESPONSE_COMBINED, COLOR_STEP_RESPONSE_HIGH_SP, COLOR_STEP_RESPONSE_LOW_SP, - FINAL_NORMALIZED_STEADY_STATE_TOLERANCE, LINE_WIDTH_PLOT, POST_AVERAGING_SMOOTHING_WINDOW, - RESPONSE_LENGTH_S, STEADY_STATE_END_S, STEADY_STATE_START_S, + FINAL_NORMALIZED_STEADY_STATE_TOLERANCE, LINE_WIDTH_PLOT, + LOW_AUTHORITY_SETPOINT_THRESHOLD_DEG_S, POST_AVERAGING_SMOOTHING_WINDOW, RESPONSE_LENGTH_S, + STEADY_STATE_END_S, STEADY_STATE_START_S, TD_COEFFICIENT_OF_VARIATION_MAX, }; use crate::data_analysis::calc_step_response; // For average_responses and moving_average_smooth_f64 +use crate::data_analysis::optimal_p_estimation::{OptimalPAnalysis, PRecommendation}; use crate::data_input::pid_metadata::PidMetadata; use crate::plot_framework::{draw_stacked_plot, PlotSeries}; use crate::types::{AllStepResponsePlotData, StepResponseResults}; -#[allow(clippy::too_many_arguments)] +/// P:D ratio recommendations with computed D values +#[derive(Debug, Clone)] +pub struct PdRecommendations { + pub pd_ratios: [Option; AXIS_COUNT], + pub d_values: [Option; AXIS_COUNT], + pub d_min_values: [Option; AXIS_COUNT], + pub d_max_values: [Option; AXIS_COUNT], +} + +/// Conservative P:D ratio recommendations (safer, incremental tuning) +#[derive(Debug, Clone)] +pub struct ConservativeRecommendations(pub PdRecommendations); + +impl std::ops::Deref for ConservativeRecommendations { + type Target = PdRecommendations; + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +/// Moderate P:D ratio recommendations (aggressive, experienced pilots) +#[derive(Debug, Clone)] +pub struct ModerateRecommendations(pub PdRecommendations); + +impl std::ops::Deref for ModerateRecommendations { + type Target = PdRecommendations; + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +/// Current peak values and P:D ratios from step response analysis +#[derive(Debug, Clone)] +pub struct CurrentPeakAndRatios { + pub peak_values: [Option; AXIS_COUNT], + pub pd_ratios: [Option; AXIS_COUNT], + pub assessments: [Option<&'static str>; AXIS_COUNT], +} + +/// Plot display configuration +#[derive(Debug, Clone)] +pub struct PlotDisplayConfig { + pub has_nonzero_f_term: [bool; AXIS_COUNT], + pub setpoint_threshold: f64, + pub show_legend: bool, +} + +/// Optional optimal P estimation analysis results +#[derive(Debug, Clone)] +pub struct OptimalPConfig { + pub analyses: [Option; AXIS_COUNT], + pub skip_reasons: [Option; AXIS_COUNT], +} + /// Generates the Stacked Step Response Plot (Blue, Orange, Red) +/// +/// Parameters are grouped logically (input data, metadata, display options, and analysis results), +/// making further consolidation impractical without reducing API clarity. +#[allow(clippy::too_many_arguments)] pub fn plot_step_response( step_response_results: &StepResponseResults, root_name: &str, sample_rate: Option, - has_nonzero_f_term_data: &[bool; 3], - setpoint_threshold: f64, - show_legend: bool, pid_metadata: &PidMetadata, - peak_values: &[Option; 3], - current_pd_ratios: &[Option; 3], - assessments: &[Option<&str>; 3], - recommended_pd_conservative: &[Option; 3], - recommended_d_conservative: &[Option; 3], - recommended_d_min_conservative: &[Option; 3], - recommended_d_max_conservative: &[Option; 3], - recommended_pd_aggressive: &[Option; 3], - recommended_d_aggressive: &[Option; 3], - recommended_d_min_aggressive: &[Option; 3], - recommended_d_max_aggressive: &[Option; 3], + current: &CurrentPeakAndRatios, + conservative: &ConservativeRecommendations, + moderate: &ModerateRecommendations, + display: &PlotDisplayConfig, + optimal_p: &OptimalPConfig, ) -> Result<(), Box> { + // Show optimal-P section whenever at least one axis has a result OR a skip reason. + let estimate_optimal_p = optimal_p.analyses.iter().any(|a| a.is_some()) + || optimal_p.skip_reasons.iter().any(|r| r.is_some()); + let step_response_plot_duration_s = RESPONSE_LENGTH_S; let steady_state_start_s_const = STEADY_STATE_START_S; // from constants let steady_state_end_s_const = STEADY_STATE_END_S; // from constants @@ -46,9 +102,10 @@ pub fn plot_step_response( let color_low_sp: RGBColor = *COLOR_STEP_RESPONSE_LOW_SP; let line_stroke_plot = LINE_WIDTH_PLOT; - let output_file_step = if show_legend { + let output_file_step = if display.show_legend { format!( - "{root_name}_Step_Response_stacked_plot_{step_response_plot_duration_s}s_{setpoint_threshold}dps.png" + "{root_name}_Step_Response_stacked_plot_{step_response_plot_duration_s}s_{}dps.png", + display.setpoint_threshold ) } else { format!("{root_name}_Step_Response_stacked_plot_{step_response_plot_duration_s}s.png") @@ -70,7 +127,7 @@ pub fn plot_step_response( AXIS_NAMES.len(), usize::min( step_response_results.len(), - usize::min(has_nonzero_f_term_data.len(), plot_data_per_axis.len()), + usize::min(display.has_nonzero_f_term.len(), plot_data_per_axis.len()), ), ); @@ -104,14 +161,14 @@ pub fn plot_step_response( } let low_mask: Array1 = valid_window_max_setpoints.mapv(|v| { - if v.abs() < setpoint_threshold as f32 { + if v.abs() < display.setpoint_threshold as f32 { 1.0 } else { 0.0 } }); let high_mask: Array1 = valid_window_max_setpoints.mapv(|v| { - if v.abs() >= setpoint_threshold as f32 { + if v.abs() >= display.setpoint_threshold as f32 { 1.0 } else { 0.0 @@ -202,7 +259,18 @@ pub fn plot_step_response( }; let mut series = Vec::new(); - if show_legend { + + // Compute the combined response once (used in both branches with different labels) + let final_combined_response = process_response( + &combined_mask, + valid_stacked_responses, + response_length_samples, + current_ss_start_idx, + current_ss_end_idx, + post_averaging_smoothing_window, + ); + + if display.show_legend { let final_low_response = process_response( &low_mask, valid_stacked_responses, @@ -219,15 +287,6 @@ pub fn plot_step_response( current_ss_end_idx, post_averaging_smoothing_window, ); - // The "Combined" response uses all QC'd windows. - let final_combined_response = process_response( - &combined_mask, - valid_stacked_responses, - response_length_samples, - current_ss_start_idx, - current_ss_end_idx, - post_averaging_smoothing_window, - ); if let Some(resp) = final_low_response { let peak_val_opt = calc_step_response::find_peak_value(&resp); @@ -245,7 +304,8 @@ pub fn plot_step_response( .map(|(&t, &v)| (t, v)) .collect(), label: format!( - "< {setpoint_threshold} deg/s (Peak: {peak_str}, Td: {latency_str})" + "< {} deg/s (Peak: {peak_str}, Td: {latency_str})", + display.setpoint_threshold ), color: color_low_sp, stroke_width: line_stroke_plot, @@ -267,15 +327,16 @@ pub fn plot_step_response( .map(|(&t, &v)| (t, v)) .collect(), label: format!( - "\u{2265} {setpoint_threshold} deg/s (Peak: {peak_str}, Td: {latency_str})" + "\u{2265} {} deg/s (Peak: {peak_str}, Td: {latency_str})", + display.setpoint_threshold ), color: color_high_sp, stroke_width: line_stroke_plot, }); } - if let Some(resp) = final_combined_response { - let peak_val_opt = calc_step_response::find_peak_value(&resp); - let latency_opt = calc_step_response::calculate_delay_time(&resp, sr); + if let Some(resp) = &final_combined_response { + let peak_val_opt = calc_step_response::find_peak_value(resp); + let latency_opt = calc_step_response::calculate_delay_time(resp, sr); let peak_str = peak_val_opt.map_or_else(|| "N/A".to_string(), |p| format!("{p:.2}")); let latency_str = latency_opt.map_or_else( @@ -295,17 +356,9 @@ pub fn plot_step_response( } } else { // If not showing legend, only plot the "Combined" (average of all QC'd responses) - let final_combined_response = process_response( - &combined_mask, - valid_stacked_responses, - response_length_samples, - current_ss_start_idx, - current_ss_end_idx, - post_averaging_smoothing_window, - ); - if let Some(resp) = final_combined_response { - let peak_val_opt = calc_step_response::find_peak_value(&resp); - let latency_opt = calc_step_response::calculate_delay_time(&resp, sr); + if let Some(resp) = &final_combined_response { + let peak_val_opt = calc_step_response::find_peak_value(resp); + let latency_opt = calc_step_response::calculate_delay_time(resp, sr); let peak_str = peak_val_opt.map_or_else(|| "N/A".to_string(), |p| format!("{p:.2}")); let latency_str = latency_opt.map_or_else( @@ -342,10 +395,17 @@ pub fn plot_step_response( // Add current P:D ratio with quality assessment as legend entries for Roll/Pitch if axis_index < 2 { + // Low-authority flight check: max setpoint across all valid windows. + let max_sp = valid_window_max_setpoints + .iter() + .cloned() + .fold(0.0_f32, f32::max); + let is_low_authority = max_sp < LOW_AUTHORITY_SETPOINT_THRESHOLD_DEG_S; + // Current P:D ratio and assessment - if let Some(current_pd) = current_pd_ratios[axis_index] { - let current_label = if let Some(assessment) = assessments[axis_index] { - if let Some(peak) = peak_values[axis_index] { + if let Some(current_pd) = current.pd_ratios[axis_index] { + let current_label = if let Some(assessment) = current.assessments[axis_index] { + if let Some(peak) = current.peak_values[axis_index] { format!( "Current P:D={:.2} (Peak={:.2}, {})", current_pd, peak, assessment @@ -364,19 +424,31 @@ pub fn plot_step_response( }); } + if is_low_authority { + series.push(PlotSeries { + data: vec![], + label: format!( + "[LOW AUTHORITY] max={:.0}dps — recommendations unreliable", + max_sp + ), + color: COLOR_OPTIMAL_P_WARNING, // Orange warning + stroke_width: 0, + }); + } + // Conservative recommendation (uses dmax_enabled computed at function start) - if let Some(rec_pd) = recommended_pd_conservative[axis_index] { + if let Some(rec_pd) = conservative.0.pd_ratios[axis_index] { let recommendation_label = if dmax_enabled { // D-Min/D-Max enabled: show D-Min and D-Max, NOT base D - let d_min_str = recommended_d_min_conservative[axis_index] + let d_min_str = conservative.0.d_min_values[axis_index] .map_or("N/A".to_string(), |v| v.to_string()); - let d_max_str = recommended_d_max_conservative[axis_index] + let d_max_str = conservative.0.d_max_values[axis_index] .map_or("N/A".to_string(), |v| v.to_string()); format!( "Recommendation (conservative): P:D={:.2} (D-Min≈{}, D-Max≈{})", rec_pd, d_min_str, d_max_str ) - } else if let Some(rec_d) = recommended_d_conservative[axis_index] { + } else if let Some(rec_d) = conservative.0.d_values[axis_index] { // D-Min/D-Max disabled: show only base D format!( "Recommendation (conservative): P:D={:.2} (D≈{})", @@ -391,7 +463,7 @@ pub fn plot_step_response( color: RGBColor(100, 100, 100), // Medium gray for conservative stroke_width: 0, // Invisible legend line }); - } else if assessments[axis_index] == Some("Near optimal") { + } else if current.assessments[axis_index] == Some("Near optimal") { // Near optimal (1.00–1.02): D−1 hint replaces the "none" label — // showing "none" and a concrete suggestion together is contradictory. if let Some(axis_pid_data) = pid_metadata.get_axis(axis_index) { @@ -436,19 +508,19 @@ pub fn plot_step_response( }); } - // Secondary recommendation (always moderate) - if let Some(rec_pd) = recommended_pd_aggressive[axis_index] { + // Moderate recommendation + if let Some(rec_pd) = moderate.0.pd_ratios[axis_index] { let recommendation_label = if dmax_enabled { // D-Min/D-Max enabled: show D-Min and D-Max, NOT base D - let d_min_str = recommended_d_min_aggressive[axis_index] + let d_min_str = moderate.0.d_min_values[axis_index] .map_or("N/A".to_string(), |v| v.to_string()); - let d_max_str = recommended_d_max_aggressive[axis_index] + let d_max_str = moderate.0.d_max_values[axis_index] .map_or("N/A".to_string(), |v| v.to_string()); format!( "Recommendation (moderate): P:D={:.2} (D-Min≈{}, D-Max≈{})", rec_pd, d_min_str, d_max_str ) - } else if let Some(rec_d) = recommended_d_aggressive[axis_index] { + } else if let Some(rec_d) = moderate.0.d_values[axis_index] { // D-Min/D-Max disabled: show only base D format!("Recommendation (moderate): P:D={:.2} (D≈{})", rec_pd, rec_d) } else { @@ -462,8 +534,8 @@ pub fn plot_step_response( }); // Aggressive (third) recommendation for significant overshoot only - if assessments[axis_index] == Some("Significant overshoot") { - if let Some(current_pd) = current_pd_ratios[axis_index] { + if current.assessments[axis_index] == Some("Significant overshoot") { + if let Some(current_pd) = current.pd_ratios[axis_index] { let aggressive_pd = current_pd * crate::constants::PD_RATIO_AGGRESSIVE_MULTIPLIER; if let Some(axis_pid_data) = pid_metadata.get_axis(axis_index) { @@ -498,6 +570,181 @@ pub fn plot_step_response( } } } + + // Optimal P estimation results (if enabled and available) + if estimate_optimal_p { + if let Some(analysis) = &optimal_p.analyses[axis_index] { + // Add separator line + series.push(PlotSeries { + data: vec![], + label: "─────────────────────".to_string(), + color: COLOR_OPTIMAL_P_DIVIDER, + stroke_width: 0, + }); + + // Optimal P header + series.push(PlotSeries { + data: vec![], + label: "Optimal P (Experimental, log-derived)".to_string(), + color: COLOR_OPTIMAL_P_HEADER, // Blue for section header + stroke_width: 0, + }); + + // Td measurement + series.push(PlotSeries { + data: vec![], + label: format!( + " Td: {:.1}ms (target: {:.1}ms, windows={})", + analysis.td_stats.mean_ms, + analysis.td_target_ms, + analysis.td_stats.num_samples + ), + color: COLOR_OPTIMAL_P_TEXT, + stroke_width: 0, + }); + + // Deviation: only prefix '+' for strictly positive deviations + let deviation_sign = if analysis.td_deviation_percent > 0.0 { + "+" + } else { + "" + }; + series.push(PlotSeries { + data: vec![], + label: format!( + " Deviation: {}{:.1}% ({})", + deviation_sign, + analysis.td_deviation_percent, + analysis.td_deviation.name() + ), + color: COLOR_OPTIMAL_P_TEXT, + stroke_width: 0, + }); + + // Noise level + series.push(PlotSeries { + data: vec![], + label: format!(" Noise: {}", analysis.noise_level.name()), + color: COLOR_OPTIMAL_P_TEXT, + stroke_width: 0, + }); + + // Consistency — always shown; orange warning when poor + { + let cv_percent = analysis + .td_stats + .coefficient_of_variation + .map_or(0.0, |cv| cv * 100.0); + let consistency_pct = + (analysis.td_stats.consistency * 100.0).round() as u32; + let (cons_label, cons_color) = if !analysis.td_stats.is_consistent() { + ( + format!( + " Consistency: {}% (CV={:.1}%) — unreliable (>{:.0}%)", + consistency_pct, + cv_percent, + TD_COEFFICIENT_OF_VARIATION_MAX * 100.0 + ), + COLOR_OPTIMAL_P_WARNING, + ) + } else { + ( + format!(" Consistency: {}%", consistency_pct), + COLOR_OPTIMAL_P_TEXT, + ) + }; + series.push(PlotSeries { + data: vec![], + label: cons_label, + color: cons_color, + stroke_width: 0, + }); + } + // Recommendation summary + // Helper closure to compute D recommendation suffix. + // Prefers the step-response conservative P:D; falls back to the + // current P:D ratio so D is always shown when D gain is known. + let effective_pd = analysis.recommended_pd_conservative.or_else(|| { + analysis + .current_d + .filter(|&d| d > 0) + .map(|d| analysis.current_p as f64 / d as f64) + }); + let append_d_recommendation = |recommended_p: u32| -> String { + if let (Some(current_d), Some(rec_pd)) = + (analysis.current_d, effective_pd) + { + if rec_pd > 0.0 && current_d > 0 { + let recommended_d = + ((recommended_p as f64) / rec_pd).round() as u32; + let d_delta = (recommended_d as i64) - (current_d as i64); + return format!(", D≈{} ({:+})", recommended_d, d_delta); + } + } + String::new() + }; + + let rec_summary = match &analysis.recommendation { + PRecommendation::Increase { conservative_p, .. } => { + let p_delta = + (*conservative_p as i64) - (analysis.current_p as i64); + let mut rec = format!( + " Recommendation (Conservative): P≈{} ({:+})", + conservative_p, p_delta + ); + rec.push_str(&append_d_recommendation(*conservative_p)); + rec + } + PRecommendation::Optimal { .. } => { + format!( + " Recommendation: Current P is optimal (P = {})", + analysis.current_p + ) + } + PRecommendation::Decrease { recommended_p, .. } => { + let p_delta = (*recommended_p as i64) - (analysis.current_p as i64); + let mut rec = format!( + " Recommendation: P≈{} ({:+})", + recommended_p, p_delta + ); + rec.push_str(&append_d_recommendation(*recommended_p)); + + rec + } + PRecommendation::Investigate { .. } => { + " Recommendation: See console output for details".to_string() + } + }; + series.push(PlotSeries { + data: vec![], + label: rec_summary, + color: COLOR_OPTIMAL_P_RECOMMENDATION, // Green for recommendation + stroke_width: 0, + }); + } else if let Some(skip_reason) = &optimal_p.skip_reasons[axis_index] { + // Show skip reason if analysis failed but we have a reason + series.push(PlotSeries { + data: vec![], + label: "─────────────────────".to_string(), + color: COLOR_OPTIMAL_P_DIVIDER, + stroke_width: 0, + }); + + series.push(PlotSeries { + data: vec![], + label: "Optimal P (Experimental, log-derived)".to_string(), + color: COLOR_OPTIMAL_P_HEADER, + stroke_width: 0, + }); + + series.push(PlotSeries { + data: vec![], + label: format!(" {}", skip_reason), + color: COLOR_OPTIMAL_P_SKIP, + stroke_width: 0, + }); + } + } } // Store title for later use @@ -509,7 +756,7 @@ pub fn plot_step_response( title.push_str(&pid_info); } } - if has_nonzero_f_term_data[axis_index] { + if display.has_nonzero_f_term[axis_index] { title.push_str(" - Invalid due to Feed-Forward"); } @@ -520,10 +767,12 @@ pub fn plot_step_response( // Calculate unified Y-axis range across ALL axes for symmetric scaling (issue #115) let (final_resp_min, final_resp_max) = if global_resp_min.is_finite() && global_resp_max.is_finite() { - // Simple symmetric range expansion with 10% padding + // Simple symmetric range expansion with 10% TOTAL padding (~5% per side) let range = (global_resp_max - global_resp_min).max(0.1); let mid = (global_resp_max + global_resp_min) / 2.0; - let half_range = range * 0.55; // 10% padding = 1.1/2 = 0.55 + // half_range = range * 0.55 gives a total span of 1.1*range (10% total expansion) + // which corresponds to ≈5% padding on each side of the center. + let half_range = range * 0.55; (mid - half_range, mid + half_range) } else { // Default range if no valid data