diff --git a/OVERVIEW.md b/OVERVIEW.md index 6d64b2e..31a9e50 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) @@ -189,6 +190,66 @@ 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-aware P gain optimization based on response timing analysis: + +- **Activation:** Disabled by default; enable with `--estimate-optimal-p` flag +- **⚠️ Status:** This feature is **experimental**. Frame-class Td targets are provisional empirical estimates requiring flight validation. Use as initial guidelines only; validation data collection is ongoing. +- **Prop Size Selection:** Use `--prop-size ` to specify **propeller diameter** in inches (1-15, integer values only). **This flag is required when `--estimate-optimal-p` is used.** + - **Critical:** Must match your actual prop size exactly (e.g., 6" frame with 5" props → use `--prop-size 5`) + - Supports whole numbers 1 through 15 only + - **No default is assumed** — you must explicitly specify the prop size. This prevents misleading recommendations when the wrong default is used. + - Prop size is a proxy for rotational inertia (I ∝ mass × radius²) which directly affects response time + - Each prop size has empirically-derived Td (time to 50%) targets based on observed flight data +- **Theory Foundation:** Based on BrianWhite's (PIDtoolbox author) insight that optimal response timing is aircraft-specific, not universal. + - **Theoretical Principle:** The relationship between response time and rotational inertia is approximate heuristic expressed as **Td ∝ √(I/τ)**, where I is the total rotational inertia (moment of inertia) of the airframe and τ is the available motor torque at the operating point (accounting for battery voltage, propeller aerodynamic load, and ESC response characteristics). This heuristic suggests that faster-rotating airframes (lower I) or higher available torque achieve quicker response times, while heavier/larger frames (higher I) or lower available torque naturally respond more slowly. + - **Real-World Factors:** In practice, Td is modified by many physical parameters beyond this simplified heuristic: mass distribution across the frame, motors, battery, and propeller placement; motor torque characteristics and efficiency across throttle range; propeller aerodynamic loading and blade pitch; battery voltage and sag during maneuvers; and ESC throttle response lag. Rotational inertia (influenced by mass × radius²) and propeller size both contribute significantly to these variations. + - **Empirical Approach:** The frame-class targets below are **empirical estimates derived from flight data**, not pure physics calculations. Propeller size is used as a practical proxy for rotational inertia because it correlates strongly with frame mass and arm length. Targets must be validated against actual flight logs for each specific build configuration, as the theoretical model cannot account for all real-world complexities. +- **Frame-Class Targets (Provisional — `TD_TARGETS` in `src/constants.rs` is authoritative):** + - Targets below are ±25% acceptance bands for pilots. If measured Td falls within target ± tolerance, the tune is acceptable. These are NOT measurement uncertainty values. + + | Prop Size | Frame Type | Target Td | Tolerance | + |-----------|------------|-----------|-----------| + | 1" | tiny whoop | 40 ms | ± 10.0 ms | + | 2" | micro | 35 ms | ± 8.75 ms | + | 3" | toothpick/cinewhoop | 30 ms | ± 7.5 ms | + | 4" | racing | 25 ms | ± 6.25 ms | + | 5" | freestyle/racing | 20 ms | ± 5.0 ms | + | 6" | long-range | 28 ms | ± 7.0 ms | + | 7" | long-range | 37.5 ms | ± 9.375 ms | + | 8" | long-range | 47 ms | ± 11.75 ms | + | 9" | cinelifter | 56 ms | ± 14.0 ms | + | 10" | cinelifter | 65 ms | ± 16.25 ms | + | 11" | heavy-lift | 75 ms | ± 18.75 ms | + | 12" | heavy-lift | 85 ms | ± 21.25 ms | + | 13" | heavy-lift | 95 ms | ± 23.75 ms | + | 14" | heavy-lift | 105 ms | ± 26.25 ms | + | 15" | heavy-lift | 115 ms | ± 28.75 ms | + + - **Reading Td deviations:** Faster than target + low noise = headroom for P increase; slower + high noise = mechanical issues or wrong prop size; within target + high noise = P at physical limits. + - **Developer validation:** A stricter ±10% statistical criterion (across ≥10 flights per frame class) is used to confirm that TD_TARGETS predictions match actual measurements. See GitHub issues for current validation status. +- **Analysis Components:** + - Collects individual Td measurements from all valid step response windows + - Calculates response consistency metrics (mean, std dev, coefficient of variation) + - Compares measured Td against frame-class targets + - Classifies Td deviation (significantly slower, moderately slower, within target, faster) + - Provides P gain recommendations based on deviation and noise levels +- **Recommendation Types:** + - **P Increase:** When Td is slower than target with acceptable noise levels + - **Optimal:** When Td is within target range or at physical limits + - **P Decrease:** When Td is faster than target with high noise (rare) + - **Investigate:** When measurements suggest mechanical issues or incorrect frame class +- **Output Format:** Detailed console report with: + - Current P gain and measured Td statistics + - Frame class comparison and deviation percentage + - Physical limit indicators (response speed, noise level, consistency) + - Clear recommendation with reasoning +- **Relationship to P:D Recommendations:** + - P:D ratio recommendations (existing feature): Analyze peak overshoot → adjust D-term + - Optimal P estimation (new feature): Analyze response timing → adjust P magnitude + - Both features are complementary and can run simultaneously for complete tuning guidance + ### 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..336d555 100644 --- a/README.md +++ b/README.md @@ -48,6 +48,8 @@ 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 with frame-class targets (requires --prop-size). + --prop-size : Propeller diameter in inches (1-15, whole-number only). Required with --estimate-optimal-p. === GENERAL === @@ -76,6 +78,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 --prop-size 5 +``` ### Output 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 6f1f041..a5d0f71 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -259,4 +259,87 @@ 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.85; // 85% of responses should be within ±1 std dev +pub const TD_COEFFICIENT_OF_VARIATION_MAX: f64 = 0.20; // 20% CV (std/mean) is acceptable + +// 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. + +/// 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; + +/// Samples to skip at the start of the response window (ESC + motor latency). +pub const TORQUE_PROFILER_SETTLE_SAMPLES: usize = 3; + +/// 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. Starting value: 2.50. +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..c2f06f0 --- /dev/null +++ b/src/data_analysis/optimal_p_estimation.rs @@ -0,0 +1,594 @@ +// 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 { + 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), Noise={}, Consistency={:.0}%\n", + axis_name, + self.td_stats.mean_ms, + target_display, + self.td_deviation_percent, + 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!( + " ⚠ WARNING: Low consistency (CV={:.1}%, {}/{} responses) - results may be unreliable\n", + cv_percent, + (self.td_stats.consistency * self.td_stats.num_samples as f64).round() as usize, + self.td_stats.num_samples + )); + } + + // 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 using recommended P:D ratio (not current ratio!) + if let (Some(current_d), Some(rec_pd)) = + (self.current_d, self.recommended_pd_conservative) + { + 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 using recommended P:D ratio (not current ratio!) + // For decrease, use conservative ratio (safer) + if let (Some(current_d), Some(rec_pd)) = + (self.current_d, self.recommended_pd_conservative) + { + 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/tests_optimal_p.rs b/src/data_analysis/tests_optimal_p.rs new file mode 100644 index 0000000..be4cf98 --- /dev/null +++ b/src/data_analysis/tests_optimal_p.rs @@ -0,0 +1,48 @@ +#[cfg(test)] +mod tests { + use crate::data_analysis::optimal_p_estimation::{FrameClass, TdTargetSpec}; + + const FRAME_SIZE_SMALL: u8 = 1; + const FRAME_SIZE_MEDIUM: u8 = 5; + const FRAME_SIZE_LARGE: u8 = 15; + + #[test] + fn td_target_spec_out_of_range_returns_none() { + assert!(TdTargetSpec::for_frame_inches(0).is_none()); + assert!(TdTargetSpec::for_frame_inches(16).is_none()); + } + + #[test] + fn frame_class_td_target_is_some_for_valid_classes() { + assert!(FrameClass::OneInch.td_target().is_some()); + assert!(FrameClass::FiveInch.td_target().is_some()); + assert!(FrameClass::FifteenInch.td_target().is_some()); + } + + #[test] + fn td_target_spec_valid_range_returns_some() { + // Representative in-range values should return Some(TdTargetSpec) + assert!(TdTargetSpec::for_frame_inches(FRAME_SIZE_SMALL).is_some()); + assert!(TdTargetSpec::for_frame_inches(FRAME_SIZE_MEDIUM).is_some()); + assert!(TdTargetSpec::for_frame_inches(FRAME_SIZE_LARGE).is_some()); + } + + #[test] + fn td_target_spec_returns_expected_values() { + // Check FrameClass td_target matches constants for key sizes + let (t1, tol1) = FrameClass::OneInch.td_target().unwrap(); + let spec1 = TdTargetSpec::for_frame_inches(FRAME_SIZE_SMALL).unwrap(); + assert_eq!(t1, spec1.target_ms); + assert_eq!(tol1, spec1.tolerance_ms); + + let (t5, tol5) = FrameClass::FiveInch.td_target().unwrap(); + let spec5 = TdTargetSpec::for_frame_inches(FRAME_SIZE_MEDIUM).unwrap(); + assert_eq!(t5, spec5.target_ms); + assert_eq!(tol5, spec5.tolerance_ms); + + let (t15, tol15) = FrameClass::FifteenInch.td_target().unwrap(); + let spec15 = TdTargetSpec::for_frame_inches(FRAME_SIZE_LARGE).unwrap(); + assert_eq!(t15, spec15.target_ms); + assert_eq!(tol15, spec15.tolerance_ms); + } +} diff --git a/src/data_analysis/torque_inertia_profiler.rs b/src/data_analysis/torque_inertia_profiler.rs new file mode 100644 index 0000000..b4e4985 --- /dev/null +++ b/src/data_analysis/torque_inertia_profiler.rs @@ -0,0 +1,234 @@ +// 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::constants::{ + THROTTLE_PUNCH_MIN_DELTA, THROTTLE_PUNCH_WINDOW_MS, THROTTLE_RESPONSE_WINDOW_MS, + TORQUE_PROFILER_MIN_CMD_DELTA_NORMALIZED, TORQUE_PROFILER_MIN_EVENTS, TORQUE_PROFILER_P_SCALE, + TORQUE_PROFILER_SETTLE_SAMPLES, 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; 3], +} + +impl AircraftProfile { + /// Build a profile from pre-collected per-axis ratio vectors. + pub fn from_axis_ratios(axis_ratios: [Vec; 3]) -> 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; 3] { + let mut axis_ratios: [Vec; 3] = [Vec::new(), Vec::new(), Vec::new()]; + + if log_data.len() < 10 || sample_rate <= 0.0 { + return axis_ratios; + } + + let punch_window = ((THROTTLE_PUNCH_WINDOW_MS / 1000.0) * sample_rate).ceil() as usize; + let response_window = ((THROTTLE_RESPONSE_WINDOW_MS / 1000.0) * 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 / 1000.0; + + if delta >= THROTTLE_PUNCH_MIN_DELTA + && cmd_delta_normalized >= TORQUE_PROFILER_MIN_CMD_DELTA_NORMALIZED + { + // Punch detected at sample i. + let resp_start = (i + punch_window + TORQUE_PROFILER_SETTLE_SAMPLES).min(n - 1); + let resp_end = (resp_start + response_window).min(n - 1); + + if resp_end > resp_start + 2 { + 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.saturating_sub(1) { + 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 > 1e-9 { + 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 ec5a4a0..59e9370 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."); eprintln!(); eprintln!("=== GENERAL ==="); eprintln!(); @@ -375,16 +387,116 @@ 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 suffix (`_YYYYMMDD_HHMMSS` and everything after) from the +/// file stem to obtain a stable key that identifies the aircraft across sessions. +/// +/// Examples: +/// `EMUF_BLACKBOX_LOG_FOXEERF722V4_426_20240406_132335_notes.19.csv` +/// → `EMUF_BLACKBOX_LOG_FOXEERF722V4_426` +/// `BTFL_BLACKBOX_LOG_20250517_130413_STELLARH7DEV.02.csv` +/// → `BTFL_BLACKBOX_LOG` (date precedes craft name; all files group together) +/// +/// 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 { + return stem[..i].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; 3] = [Vec::new(), Vec::new(), 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 +548,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 +751,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!(); @@ -849,7 +967,7 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." let d_max_str = recommended_d_max_conservative [axis_index] .map_or("N/A".to_string(), |v| v.to_string()); - println!(" Conservative: P:D={:.2} → D-Min≈{}, D-Max≈{} (P={})", + println!(" Conservative recommendation: P:D={:.2} → D-Min≈{}, D-Max≈{} (P={})", recommended_pd_conservative[axis_index].unwrap(), d_min_str, d_max_str, p_val); } else if let Some(recommended_d) = @@ -857,7 +975,7 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." { // D-Min/D-Max disabled: show only base D println!( - " Conservative: P:D={:.2} → D≈{} (P={})", + " Conservative recommendation: P:D={:.2} → D≈{} (P={})", recommended_pd_conservative[axis_index].unwrap(), recommended_d, p_val @@ -877,7 +995,7 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." let d_max_str = recommended_d_max_aggressive [axis_index] .map_or("N/A".to_string(), |v| v.to_string()); - println!(" Moderate: P:D={:.2} → D-Min≈{}, D-Max≈{} (P={})", + println!(" Moderate recommendation: P:D={:.2} → D-Min≈{}, D-Max≈{} (P={})", recommended_pd_aggressive[axis_index].unwrap(), d_min_str, d_max_str, p_val); } else if let Some(recommended_d_mod) = @@ -885,7 +1003,7 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." { // D-Min/D-Max disabled: show only base D println!( - " Moderate: P:D={:.2} → D≈{} (P={})", + " Moderate recommendation: P:D={:.2} → D≈{} (P={})", recommended_pd_aggressive[axis_index].unwrap(), recommended_d_mod, p_val @@ -911,6 +1029,146 @@ 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, + >; 3] = [None, None, None]; + let mut optimal_p_skip_reasons: [Option; 3] = [None, None, 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() { + // 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 { + println!(" P gain not available for {axis_name}. Skipping optimal P analysis."); + } + } + } + } + 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 @@ -929,25 +1187,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, )?; } @@ -1001,7 +1287,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, )?; @@ -1013,7 +1299,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, )?; @@ -1025,7 +1311,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, )?; @@ -1052,7 +1338,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 { @@ -1115,6 +1406,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; @@ -1182,6 +1474,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); @@ -1266,29 +1560,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 a77c92c..a1c4fc7 100644 --- a/src/plot_functions/plot_step_response.rs +++ b/src/plot_functions/plot_step_response.rs @@ -4,39 +4,91 @@ 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_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, }; 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> { + // Derive estimate_optimal_p from presence of analyses (removes redundant boolean parameter) + let estimate_optimal_p = optimal_p.analyses.iter().any(|a| a.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 +98,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 +123,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 +157,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 +255,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 +283,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 +300,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 +323,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 +352,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( @@ -343,9 +392,9 @@ pub fn plot_step_response( // Add current P:D ratio with quality assessment as legend entries for Roll/Pitch if axis_index < 2 { // 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 @@ -365,22 +414,25 @@ pub fn plot_step_response( } // 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!( - "Conservative: P:D={:.2} (D-Min≈{}, D-Max≈{})", + "Conservative recommendation: 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!("Conservative: P:D={:.2} (D≈{})", rec_pd, rec_d) + format!( + "Conservative recommendation: P:D={:.2} (D≈{})", + rec_pd, rec_d + ) } else { - format!("Conservative: P:D={:.2}", rec_pd) + format!("Conservative recommendation: P:D={:.2}", rec_pd) }; series.push(PlotSeries { data: vec![], @@ -391,22 +443,22 @@ pub fn plot_step_response( } // Moderate recommendation - if let Some(rec_pd) = recommended_pd_aggressive[axis_index] { + 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!( - "Moderate: P:D={:.2} (D-Min≈{}, D-Max≈{})", + "Moderate recommendation: 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!("Moderate: P:D={:.2} (D≈{})", rec_pd, rec_d) + format!("Moderate recommendation: P:D={:.2} (D≈{})", rec_pd, rec_d) } else { - format!("Moderate: P:D={:.2}", rec_pd) + format!("Moderate recommendation: P:D={:.2}", rec_pd) }; series.push(PlotSeries { data: vec![], @@ -415,6 +467,156 @@ pub fn plot_step_response( stroke_width: 0, // Invisible legend line }); } + + // 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: RGBColor(40, 40, 40), + stroke_width: 0, + }); + + // Optimal P header + series.push(PlotSeries { + data: vec![], + label: "Optimal P (log-derived)".to_string(), + color: RGBColor(0, 100, 200), // Blue for section header + stroke_width: 0, + }); + + // Td measurement + series.push(PlotSeries { + data: vec![], + label: format!( + " Td: {:.1}ms (target: {:.1}ms)", + analysis.td_stats.mean_ms, analysis.td_target_ms + ), + color: RGBColor(80, 80, 80), + 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: RGBColor(80, 80, 80), + stroke_width: 0, + }); + + // Noise level + series.push(PlotSeries { + data: vec![], + label: format!(" Noise: {}", analysis.noise_level.name()), + color: RGBColor(80, 80, 80), + stroke_width: 0, + }); + + // Consistency (if poor, show warning) + if !analysis.td_stats.is_consistent() { + let cv_percent = analysis + .td_stats + .coefficient_of_variation + .map_or(0.0, |cv| cv * 100.0); + series.push(PlotSeries { + data: vec![], + label: format!( + " [WARNING] High variability (CV={:.1}%) - results may be unreliable", + cv_percent + ), + color: RGBColor(200, 100, 0), // Orange for warning + stroke_width: 0, + }); + } + // Recommendation summary + // Helper closure to compute D recommendation suffix + let append_d_recommendation = |recommended_p: u32| -> String { + if let (Some(current_d), Some(rec_pd)) = + (analysis.current_d, analysis.recommended_pd_conservative) + { + 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: RGBColor(0, 150, 0), // 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: RGBColor(40, 40, 40), + stroke_width: 0, + }); + + series.push(PlotSeries { + data: vec![], + label: "Optimal P (SKIPPED)".to_string(), + color: RGBColor(180, 40, 40), // Red for skipped/error + stroke_width: 0, + }); + + series.push(PlotSeries { + data: vec![], + label: format!(" {}", skip_reason), + color: RGBColor(120, 60, 60), + stroke_width: 0, + }); + } + } } // Store title for later use @@ -426,7 +628,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"); } @@ -437,10 +639,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